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I. INTRODUCTION 


A. GENERAL 


As the general downsizing of the military continues, the trend on future 
battlefields will be toward smaller units being responsible for scouting and securing larger 
areas. It is also predicted that, in the near future, 70 percent of the world’s population 
will be in urban areas [Ref. 1]. As more and more of the general population moves into 
cities, future battlefields are more likely to be in urban or built-up areas. As these trends 
continue the need for some type of robotic support and augmentation for the small unit or 
individual on the ground will become greater. 

Urban and built-up areas present some of the greatest challenges for military units 
in the areas of initial reconnaissance, continued observation, and control of lines of 
communication and other key terrain features. Multisensor systems, mounted on a 
variety of robotic platforms, can provide this type of battlefield support in areas where it 
is needed most. However, before making very costly decisions about the make-up of 
these systems it is imperative to conduct some basic research about the types of systems 
that are most cost effective and most efficient. This will allow the system designers to 
make intelligent decisions about the type and composition of systems that will be most 


useful on tomorrow’s battlefield. 


B. PROBLEM STATEMENT 


As a reconnaissance system is designed there are several fundamental questions 
that must be asked and answered. In some missions, a large number of the simplest 
possible systems may be the right answer. This might be the case if all that is desired is 
simple detection of “something” with no detail other than the fact that there is 
“something” in the vicinity of the systems sensors. For other missions the best solution 
may be a smaller number of systems incorporating higher capability sensors, increased 
processing capability, improved communications resources, and greater mobility. This 
would be the case if the system were required to perform more complex tasks such as 
target identification, target tracking, or even target attack. Or perhaps the best system for 
the mission lies somewhere in-between these two extremes or is even a combination of 
them both. [Ref. 2] 

This thesis will explore a comparison between the first and second options. A 
robotic mapping system was originally developed for a small number of very expensive, 
but very sensor capable, NOMAD 200 robots. By taking this same system and 
implementing it on a larger number of much less expensive, but less capable, NOMAD 
SCOUT robots the beginnings of a comparison between the two options will be possible. 

In addition, many of the challenges and questions inherent to the development of a 
multiple robot mapping system are also present in any research involving multiple robots 
attempting to accomplish a common task. The problems of communication, coordination, 


and control apply similarly to multiple robot mine clearing systems and robotic weapon 


platforms. It is hoped that the development of a multiple robot testbed can set the stage 


for further research in these areas at this institution. 


c: OUTLINE OF THE THESIS 


Chapter II provides a description of the platforms and sensors that were used for 
the research in this thesis, as well as the platforms used in previous work for comparison 
purposes. Chapter III discusses the other hardware and software common to this and 
other research that was used to provide connectivity and control within the systems. 
Chapter IV provides background on the techniques of robotic map building. Chapter V 
describes the exploration strategies and techniques used in this and other studies. Chapter 
VI describes the methods of integrating the work of multiple robots in a cooperative map 
making effort. Chapter VII presents the results that were obtained at the Naval 
Postgraduate School (NPS) based upon the system originally designed at the Naval 
Research Laboratory (NRL). Finally, Chapter VIII discusses the conclusions and 


recommendations for follow-on studies. 





I. PLATFORMS AND SENSORS 
This chapter provides background information so that the reader has an 
understanding of the NOMAD 200 and NOMAD SCOUT mobile robots and the 
similarities and differences between these two platforms. It will also describe the sensors 
available on each of the platforms, as well as some details concerning previous work done 
with the NOMAD 200 at both NPS and NRL. Figure | provides a relative size and 


shape comparison of the NOMAD 200 and NOMAD SCOUT. 
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Figure 1. Relative size and shape comparison of the NOMAD SCOUT (left) and NOMAD 
200 (right) — note telephone book in front of NOMAD 200 for reference. 


A. NOMAD 200 MOBILE ROBOT 


The NOMAD 200 1s an integrated mobile robot system with four sensory 
modules including tactile, infrared, sonar and laser sensors. There are multiple onboard 
computers that provide sensor and motor control, as well as providing communication to 
the host computer via a wireless Ethernet system [Ref. 3]. Figure 2 provides a detailed 


picture of the NOMAD 200. 


1. Mechanical Description 


The NOMAD 200 base chassis is driven by a three-wheel synchronous drive 
mechanism, using one motor to drive all of the wheels and a second motor to steer all of 
the wheels. The robot has a zero gyro-radius, meaning that it can rotate about its own 
center. It can translate at up to 24 inches per second and rotate at a maximum rate of 60° 
per second. The base is 18 inches in diameter, which extends to 21 inches with the 
bumper installed. The NOMAD 200 stands 31 inches tall, excluding any additional 
sensors added on top of the robot [Ref. 4]. The turret (which all the sensor systems are 


mounted on) can be rotated independently of the base. [Ref. 4] 


Ze Sensor Systems 


The basic NOMAD 200 sensor array includes tactile (bumper) sensors, infrared 
sensors, sonar sensors, and laser sensors. In addition to these sensor systems, the robot 


also has an odometric system that tracks the robot’s movements. The encoder resolution 


on this odometric system is 18 counts/cm for translation and 1510 counts/degree for robot 


steering and movement of the turret. 





Figure 2. Detailed close-up picture of the NOMAD 200. 


The tactile system consists of a bumper ring positioned over 20 independent 
pressure-sensitive sensors. These simple on-off sensors are interleaved around the 
bumper ring in order to provide 360° coverage with 18° resolution. [Ref. 4] 

The infrared sensors aboard the NOMAD 200 incorporate a 16 channel, reflective 
intensity based, infrared ranging system that provides 360° of coverage. Each of the 16 
sensors is composed of two light emitting diode (LED) emitters and a photodiode 
detector enclosed in a delrin housing. The range from the sensor to the object(s) is 
determined by the amount of light from the emitters that is reflected back to the detectors 
after striking the object(s). The reflectivity of the object greatly affects the reading. Thus 
the system needs to be calibrated for the environment in which it is to be used. With 
appropriate calibration, range accuracy is within 5% from 0 to 24 inches from the sensor. 
[Ref: 5] 

The sonar sensors on the robot are composed of a 16 channel, time-of-flight based, 
sonar ranging system. The system uses standard Polaroid transducers. Each transducer 
has a beam width of 25°. Range from the sensor to object(s) is determined by the time of 
flight of the acoustic signal generated by the transducer and reflected back by the 
object(s). The user has the option to control the firing sequence of the 16 individual sonar 
sensors mounted about the circumference of the robot. To minimize the potential for 
sensor interaction, a non-sequential firing sequence is recommended. [Ref. 6] 

The laser sensor on the NOMAD 200 is a two-dimensional, triangulation-based 
laser ranging system. A laser diode is used as the light source and a charged-coupled 


device (CCD) array camera is used to generate an image. The laser diode produces a 


horizontal “plane” of light. The CCD camera is placed vertically above this “plane” and 
inclined downward. Any object intersecting this plane forms a light stripe on the image 
generated by the CCD camera. The range to this object is found by determining the 
position of this light stripe along the scan lines of the camera. This system has an 


operating range from 12 to 120 inches. [Ref. 7] 


a: Previous Work with this Platform 


The NOMAD 200 has been and continues to be a very popular research platform 
at NPS and elsewhere. There exists an extensive body of work involving the NOMAD 
200 in several areas in the field of robotics. Some of the more recent work at NPS has 
involved localization of the robot position in an unknown environment [Ref. 8, 9] and 
geometric formation and movement in formation of multiple robots in simulation [Ref. 3]. 
Because NPS only has a single NOMAD 200 (due primarily to the expense of the 
platform), all work involving multiple robots had to be simulated until very recently. 
This single robot limitation coupled with the high logistics cost of setting up a very 
complicated robot platform have been major factors in limiting research performed with 
real, vice simulated, robots at NPS. 

NRL has also done extensive research using the NOMAD 200. Their acquisition 
of two NOMAD 200 robots has allowed them to conduct more actual research involving 
multiple robots in addition to simulations. The basis for this thesis 1s an adaptation of 
some of their work (described below) in order to form a testbed for actual multiple robot 


work here at NPS involving less costly platforms. 


B. NOMAD SCOUT MOBILE ROBOT 


The NOMAD SCOUT 1s an integrated mobile robot system with ultrasonic and 
tactile sensors, as well as an odometric system. It uses a multiprocessor, low-level 
control system that controls the sensing, motion, and communications. At a high level, 
the SCOUT is controlled either by a laptop, mounted on top, or a remote workstation 
communicating via radio modem. The SCOUT is code compatible with NOMAD 200 
class robots [Ref. 10], which was a very important consideration in its choice as a 


research platform at NPS. Figure 3 provides a detailed picture of the NOMAD SCOUT. 


i Mechanical Description 


The NOMAD SCOUT is a two-degree of freedom (DOF) differential drive robot. 
The drive is set about the geometric center of the robot, which allows the robot to turn or 
rotate about its own axis. The NOMAD SCOUT has a maximum speed of one meter per 
second with a maximum acceleration of two meters per second squared. The robot is .38 
meters in diameter and 1s .34 meters in height. Without batteries, the unit weighs 23 


kilograms. [Ref. 11] 


2: Sensor Systems 


The basic NOMAD SCOUT sensor array includes tactile (bumper) sensors and 
sonar sensors. In addition to these sensor systems, the NOMAD SCOUT also has an 


odometric system that tracks the robot’s movements. The encoder resolution on this 


odometric system is 167 counts/cm for translation and 45 counts/degree for robot 


rotation. 
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Figure 3. Detailed close-up picture of the NOMAD SCOUT with radio modem. 
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The tactile system uses a nbbon switch enclosed in an energy absorbing neoprene 
channel to provide 360-degree coverage. The ultrasonic system uses 16 independent 
sonar sensors. The effective range of the ultrasonic sensors is 6 to 255 inches. This sonar 
sensor is basically identical to the one installed in the NOMAD 200 with slight 


differences due to the smaller diameter of the NOMAD SCOUT. [Ref. 11] 


C. COMMUNICATION AND COMPUTATIONAL RESOURCES 


No description of the platform would be complete without mention of the 
software and hardware that allow the robots to operate. A thorough knowledge of the 
underlying hardware and software is essential in understanding the model being used in 


this research. 


i Software 


The Nomadic Host Development Environment (NHDE) is a full-featured, object- 
based mobile robot software development package for both the NOMAD 200 and 
NOMAD SCOUT mobile robots [Ref. 12]. The complete package provides the control 
and graphics interfaces as well as a very realistic simulation tool for program testing. By 
using the supplied development package it is much easier to concentrate research on 
higher level issues of motion planning and control because most of the lower level issues 


of sensor and motor control are handled by the included software. 


The control interface allows for programming the NOMAD 200 or NOMAD 
SCOUT using a high-level programming language (either C, C++ or Lisp) and linking to a 
supplied library [Ref. 12]. Built-in to the supplied library are interfaces to the supplied 
driver software that handle lower level functions such as sensor and motor control. This 
allows for a higher level of abstraction in the researcher’s approach. The graphical user 
interface and simulator are accessible when the user runs the executable server program, 
Nserver [Ref. 3]. 

The graphical user interface in the NHDE is based on the OSF/Motif graphics 
toolkit for the X Window System [Ref. 12]. The graphical interface can display 
information on up to six robots simultaneously. There are several different windows 
displayed in the complete graphical user interface. First, there is the world (map) 
window which gives an overall view of the environment (real or simulated) that the robots 
are in, as well as the positions of the robots relative to the environment and one another. 
Secondly, there is a robot window, with one copy per robot, which contains information 
about each individual robot. This information includes the current command being 
executed, position and orientation information, and sensor information. Along with each 
robot window, there are two more windows that give more detailed information about 
current sensor readings. These two windows are usually used to display a graphical 
representation of the sonar and infrared returns of each robot’s sensors. Detailed 
information about each robot can be saved as a setup file (robot.setup) [Ref. 3]. This 
includes information about the model of robot (NOMAD 200 or NOMAD SCOUT) 


being used. 
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The Nomadic Simulator 1s a fully functional mobile robot simulator that can 
accurately model most environments, the robot’s motion, and its sensing capabilities. 
There is a high degree of correlation between the simulated world and the real world. Ifa 
program will not run on the simulator it definitely will not work on a real robot. The 
simulation tool allows the researcher to build a controlled environment in which to 
develop and debug programs. Using a graphical drawing tool a user can draw a map in the 
world window to simulate the desired surroundings. This file can be saved as a setup file 
for the world (world.setup). In addition, once a program is running properly on a 
simulated robot, it can be switched to a real robot via a pull-down menu within the 
simulator. Once this is done the graphical interface will then begin to display information 
from the real robot vice the simulated one while commands from the program will control 
the real robot via the server program (Nserver). [Ref. 3] 

The NHDE also incorporates several other very convenient features that aid the 
researcher. There is a record and playback tool that that allows for sensor data and/or 
executed commands to be stored for later analysis, as well as providing for an instant 
replay capability. This is an invaluable debugging tool. There is also a console available 
on Nserver that allows the user to directly input to the robot any possible command. 
This is a very handy option for checking the robot’s sensors or making small, subtle 
adjustments in the robots location during experiments. In addition, there is an on-screen, 
software joystick that allows the user to remotely drive the robot. This is often used to 


move actual robots around in the real world while simultaneously collecting sensor data. 
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This allows the researcher to write software that collects and manipulates sensor data 
without having to also write code to handle the robot’s motion. 

Figure 4 shows a typical NHDE display from an experiment involving two robots 
in a simulated environment. Also shown in the figure are many of the features described 
above such as the global map window, the robot window with its associated sensor 
windows, the record and playback window, the command console window, and the 
software joystick window. Also demonstrated in the robot window is another feature 
available in NHDE. The user has the option to display raw sensor return data or “hits” in 
the robot window as well as a copy of the global map. This provides a quick visual 
reference to the researcher indicating whether or not the robot’s sensors are functioning 
properly. 

Using Nserver in conjunction with the graphical interface and simulation tool is a 
very convenient way to test and debug software in simulation for subsequent use on a real 
robot. However, because of the client-server architecture, testing client programs on the 
real robot via Nserver may slow the control and data return rates because Nserver acts as 
a router. Once a program is working properly it can be recompiled to use the control 
interface library directly without the need for Nserver to be running concurrently. This is 
a very simple process because of the efficient design of the NHDE software. 

Each of the application programs for each robot, as well as the Nserver when 
used, can run simultaneously as separate processes under the UNIX operating system. 


All communications between the host processes that are controlling the robots are 
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handled as communications between UNIX processes using the TCP/IP protocols and a 


server-client architecture. This will be described in more detail in Chapter IV. 
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Figure 4. Typical graphical display. (From Ref. [12]) 


Zz Hardware 


As mentioned above, each program runs as a separate UNIX process. Whenever 
practical each process is run on a separate Sun workstation because the frontier-based 
exploration program is computationally demanding. In addition, running each robot with 


a separate workstation is more faithful to the system that is being modeled as described in 


Chapter IV. 
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Communications from host process to host process are handled as described 
above and in more detail in Chapter IV. Communications between the host process and 
the robot it controls are handled via radio Ethernet. Each robot has a 2.4 GHz radio 
modem. This radio modem is assigned an IP address that the host process uses to route 
instructions to the robot [Ref. 13]. The radio modem connects to a wireless access point 
that provides connectivity between the radio modem and the rest of the network [Ref. 


14]. 
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I. EVIDENCE GRID BASED MAPPING TECHNIQUES 


Over the years many methods have been developed to convert sensor data from 
robots into useful maps. At times it seems that there are as many different types of 
robotic mapping techniques as there are robots. Each group of researchers has approached 
the problem with a slightly different variation or procedure. However, one major method 


that has proven very successful is called the “grid method.” 


A. OVERVIEW OF GEOMETRIC MAPPING TECHNIQUES 


A geometric map represents objects according to their geometric relationships. It 
can be a grid map, or a more abstracted map, such as a line map or a polygon map [Ref. 
15]. A geometric map also has the advantage of being easily interpreted by humans trying 
to match the map with the area that it represents. The key 1s finding the method that 
builds the best map. 

One of the problems with building a map using simple lines and polygons is that 
the mapping capability breaks down quickly in a non-simulated environment. These 
methods depend on interpreting small amounts of sensor return data and mapping points, 
lines, or surfaces to that data. This can work very well in a simulated environment where 
obstacles tend to be composed of simple geometric shapes and straight lines, but the real 
world is not made up of such convenient shapes. Often times such methods map false 
obstacles or incorrectly shaped obstacles based on extraneous or incorrect sensor data. 


This is especially a problem when dealing with sensors that return much “noisy” data by 
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their nature, as do sonar sensors. So these line and polygon mapping techniques are said 
to be lacking robustness. 

The grid technique was developed as a way to overcome many of the problems 
described above. When using a grid method it is not necessary to make assumptions 
about the shape or size of an object being mapped. Simply plotting enough sensor return 
points on a grid forms a recognizable map that can be used by both robots and humans. 
Once enough points are plotted, edge detection techniques can then be used to pick out 
walls, obstacles, unexplored areas, and other terrain features. More about edge detection 


techniques and their uses in mapping will be discussed in Chapter IV. 


B. SIMPLE PLOTTING OF SENSOR DATA 


Perhaps the most basic of the grid methods is simply plotting the data returned by 
the robot’s sensors and marking areas within the sensors’ range as either occupied or 
unoccupied. This approach has the advantage of simplicity and can produce a reasonably 
good map in a well-defined simulated environment. However, in a real world environment 
using only sonar as a sensor this method quickly breaks down unless very tight 
constraints are set on the range of data used. This was the first mapping technique 
attempted for this research in conjunction with a single robot. Although this method was 
later abandoned in favor of a technique better suited for a multiple robot system, it 1s still 
useful in depicting some of the complexities of robotic mapping systems. 

Figures 5 and 6 illustrate a comparison of simulated and real-world maps 


constructed by the simple plotting of sonar return data with varying range limitations 
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imposed on the displayed data. Figure 5(a) is a typical simulated environment used to 
test various robotic map-making techniques. The environment is a roughly 325 by 275 
inch rectangle with several large geometrically shaped obstacles placed within it. Figure 
6(a) is a photo of an aisle between two laboratory benches that was used to test map- 
making methods in areal environment. The aisle has several chairs with metal legs along 
the robot’s projected path as well as open spaces beneath the benches. In both the 
simulated and real worlds the robot is remotely moved through the environment via the 
software joystick described above. 

At the same time the user remotely moves the robot, another process is running 
which collects the sonar return data and the robot position and orientation (pose) data and 
writes it to a data file. Pose data is very important in converting the sonar returns for a 
given robot position and orientation into data that can be mapped onto a common 
coordinate system. After the data were collected, a MATLAB routine read the data file, 
transformed the sonar return and pose data, and plotted the resulting map. Along with 
the sonar return data the robot’s path in the simulated or real world is also plotted as a 
dotted line in the resulting map. In this case the map was generated after maneuvering the 
robot, but Nserver also allows for the raw sensor returns to be plotted in real time within 
the robot window. 

In Figures 5(b) and 6(b) the reliable sonar range is set to 255 inches (the maximum 
rated reliable range according to the manufacturer’s specifications). Thus, the mapping 
program plots all sonar return data that is below 255 inches. A return of 255 inches is 


regarded as open space and not plotted. In the simulated world this produces a relatively 


a 


good map with some noise at corners and other line intersections. In the real world there 
is much noise and what appear to be many extraneous returns. This noise and the 
apparent false returns will be discussed in more detail in Chapter IV. 

In Figures 5(c-e) and 6(c-e) the reliable sonar range 1s steadily reduced and a larger 
percentage of the raw sonar returns eliminated and not plotted. Correspondingly, as the 
outlying returns are discarded, the data that is plotted produces clearer and less noisy 
maps. Unfortunately, as can be seen very well in Figure 5(e), dropping the longer returns 
in the larger simulated environment resulted in the robot path being too distant from 
several abeaele walls, preventing them from being mapped. This illustrates the tradeoff 
between reducing the reliable sonar return range in order to get quality data, and forcing 
the robot to travel further in order to close in on all mappable objects in the environment. 
More about this tradeoff will be discussed in greater detail in Chapter V. 

Plotting every sensor return does not prove to be the best method of constructing 
a useable map. The same simplicity that makes it so easy to implement also proves to be 
its downfall in real world situations. It is possible to fuse maps together with this 
method by converting all returns to a common coordinate system, but the main problem is 
that all data is given the same amount of validity. What is needed is a method to weigh, or 
measure, the “goodness” of sensor data from multiple sensors at multiple positions and 


build a map accordingly. 
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Figure 5. Illustration of simulated environment and the resulting sonar maps formed by 
maneuvering the simulated robot throughout it, collecting sonar range data, and plotting 
subsets of that range data based on estimated reliability. 
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Figure 6. Illustration of real world environment and the resulting sonar maps formed by 
maneuvering the actual robot throughout it, collecting sonar range data, and plotting 
subsets of that range data based on estimated reliability. 
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C: THE EVIDENCE GRID METHOD 


The evidence grid method was developed as a technique to create high resolution 
maps from wide-angle sonar. This approach allows range measurements from multiple 
points of view to be systematically integrated into a common map. The integration 
technique allows for multiple readings of an area to either reinforce or refute one another 
as to the whether or not the area is occupied. As more sensor data is added the definition 
of the map improves. [Ref. 16] 

In the evidence grid method, the area to be mapped 1s divided into a grid with M X 
N cells. In each of these cells is stored a set of information regarding the best estimate as 
to what that cell contains. This “evidence” can be many different things, such as the 
surface orientation or color of whatever is in the cell, but for mapping perhaps the most 
useful information is occupancy information concerning the cell [Ref. 17]. Early versions 
of this method [Ref. 16, 18] stored this occupancy information as a two part record with 
a status of either unknown, empty, or occupied and an associated certainty factor of either 
0, -1 to 0, or 0 to 1 respectively. Early versions also used ad hoc formulas to combine 
this information about each cell into a useable map. 

Later implementations [Ref. 17, 19, 20] eliminated the two part occupancy record 
and replaced it with a single value representing the probability that the cell 1s occupied. 
This technique also allowed for a better method of combining and integrating sensor data 
using a variation of Bayes theorem. In this representation, an unknown or unexplored cell 


would have an occupancy probability of 0.5. As more sensor data becomes available this 
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probability changes accordingly. The details of how this sensor data is integrated together 


are the topic of the next section. 


D. FUSING SENSOR DATA USING AN EVIDENCE GRID 


The major advantages of the evidence grid method over previous methods are the 
ability to weigh or measure the “goodness” of the sensor data and the ability to fuse this 
data in a simple, yet effective, manner. The best way to demonstrate how the sensor data 
is fused is to first present a graphical representation and then go more in depth into the 


mathematics behind it. 


i; Graphical Presentation 


Figure 7 shows a sequence of images simulating the fusion of sonar sensor data 
from two different points. In this sequence, the circles labeled A and B represent 
locations at which a sonar sensor sends out a pulse and receives a return. Points A and B 
could be different sensors on the same robot, the same sensor on a single robot at a 
different time, location and/or orientation, or two different sensors on two separate 
robots. The advantage of this sensor fusion method is that for all practical purposes, 
exactly what they are does not matter. The only thing that matters is that the readings be 
relatively independent of one another. For the purposes of this explanation, they will be 
referred to as sensor A and sensor B. The background grid will represent the evidence grid 


that will be developed in order to map the region. As per most recent implementations of 
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the evidence grid model it will be assumed that all the cells on the grid will be initialized to 
an initial occupancy probability of 0.5. 

In Figure 7(a) the relative geometric locations of A and B are shown, along with 
some object in the distance. Figure 7(b) shows a two-dimensional “slice” of a three- 
dimensional sonar cone emanating from A. R,»g4 1s the maximum effective range of sensor 
A. Roya is the range at which the object is detected some distance away from A. Because 
of the wide-angle nature of the sonar sensor the object is known to be only somewhere on 
a certain surface [Ref. 20]. In this case the dashed line represents the edge of the circular 
“slice” of the sonar cone along which the object might lie. So at this point it is known 
that an object lies somewhere within a constrained region. 

The shape of that region and its distance from A are also known, but there is not 
enough information at this point to make an assumption about exactly where in that 
region the obstacle exists. Under the evidence grid model the occupancy probabilities of 
the cells along that entire region would be increased. In the two-dimensional case this 
would mean all the cells along the dashed line. The amount of increase might vary 
depending on several factors such as distance from target, angle from sensor, or other 
measures of sensor data quality, but the main point is that the occupancy probabilities 
along the assumed obstacle location would be increased relative to the surrounding cells. 

Figure 7(c) shows the two-dimensional representation of the sonar cone emanating 
from B. Again, B may be the same sensor as A at different time, location, and/or 
Orientation or a Separate sensor on the same or a different robot. In this case B does not 


sense the obstacle within the region it is observing. 
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(d) 





Figure 7. Example of fusion of sonar return data from two geographically different sonar 
SENSOYS. 


Figure 7(d) displays both the sensor readings from A and B simultaneously. In 
this case part of the sensor reading from B overlaps some of the region of the sensor 
reading from A where the cells had had their occupancy probability raised relative to the 


surrounding cells. However, the data from sensor B now provides additional information 
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concerning portions of this area and that information can be used to adjust the occupancy 
probabilities in that region accordingly. 

Figure 7(e) shows an enlarged view of the area where the sensor readings from A 
and B intersect. In the evidence grid model the information from A and B would be 
combined in such a way as to lower the occupancy probabilities of the cells within the 
circle marked Region / that are in view from both A and B. At first 1t would seem logical 
to also increase the occupancy probabilities of those cells within the circle marked Region 
2 as well. However, because the evidence grid model used considers each sensor reading 
to be relatively independent of every other sensor reading, the reading from B cannot be 
used to adjust the occupancy probabilities of the cells in Region 2 because those cells are 
not in view of B. 

Depending on the detailed specifics of the model chosen, the occupancy 
probabilities of the cells in Region J might still be higher than their immediate neighbors 
that were always considered empty, but they would always be lower than those of the 
cells in Region 2. So even though the cells in Region 2 are not directly changed through 
this process, their occupancy values are now the local maxima for the overall area within 
the large evidence grid shown in the figure. 

Figure 7 illustrates the evidence grid method on a small scale. Now imagine it on a 
much larger scale with multiple sensors on multiple platforms and many hundreds to 
thousands of relatively independent sensor readings from a multitude of ranges and 
Orientations. It is by combining all of these together that a map is created using the 


evidence grid method. The mathematical details of this process are described below. 
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2: Mathematical Presentation 


Perhaps the clearest and most concise mathematical description of the evidence 
grid method of combining sensor data can be found in [Ref. 17]. What follows in this 
section is a condensed version of that work presented here for clarity and completeness. 


Let p(A| B) represent the best estimate of the likelihood of situation A given that 


information B has been received. A and B mean either “a certain region of space is 
occupied,” (written o), “a certain region of space is unoccupied”, (written 0), or they 
represent a sensor reading. By definition, p( A| B)= p(/AMB)/ p(B). The quantity 
p(A) represents the estimate of A given no new information. The alternative to situation A 
is written A, (read as “not A”). 

For the two occupancy cases of a cell, o (the cell is occupied) and o (the cell is 
empty), and new information M (derived from a sensor measurement), the above 


definition creates the equation: 


p(o|M) _ p(M|o) p(o) (1) 
p(o|M) p(M\|o) p(o) 


Now this can be rewritten as: 


pP(M\o) _ p(o) p(o| M) (1a) 
P(M|o) plo) p(o| M) 


Now suppose that there exists some information, M;, that has already been 
processed into a map, 1.e. p(o|M,) already exists and it is desired to integrate some new 
measurement, M>, to find p(o| M, © M,). In order to make the analysis tractable it is 


assumed that the new measurement is independent from all previous information. This 


5) 


may not be completely true, but for the purposes of constructing a map from many 
sensor inputs it simplifies the problem immensely. However, it is not implied that 
p(M,AM,)= p(M,)p(M,), since 1f M; indicates that the cell is occupied then it is 
hoped that M> would be more likely to indicate the same thing. Instead, what is meant is 
that, given that the cell is occupied, the probability of getting reading M, is independent of 
getting M>, and similarly for the cell being occupied: 

p(M,O M,|0)= p(M,|0)p(M,|o) (2) 

P(M,OM,|0)= p(M,|0)p(M,| 0) (3) 

Another way to look at this assumption is that it is only assumed that the 

sensor’s errors are independent from one reading to the next. This is especially true of 
noisy sonar sensor data, in which the errors vary greatly from one reading to another from 
the same sensor due to changes in range, orientation, etc. Combining this assumption 


with a single application of Equation 1a, results in: 


p(o| M,AM,) = p(o| M,)p(M,|o) Ss p(o| M,)p(o| M,)p(o) 
p(o|M,AM,) p(o|M,)p(M,|0) p(o|M,)p(o| M,)p(o) 


(4) 
We generally assume that the a priori probability of a cell being occupied is 0.5, 
1.e., D(0)=p(0 )=0.5, so that the last factor in Equation 4 cancels out. When the 
information M, is a sensor reading, the value p( M,|0)/ p( M,|0 ), for all cells and all 
possible readings, is called the sensor model. In other words, the sensor model is a 
function which attaches a number, ( p( M,|0)/ p( M,|0)), to every combination of 


sensor reading and cell location, relative to the sensor. This assumes that the sensor is 


isotropic in its world position and pointing direction. In general, the sensor model is a 
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function of the sensor reading, the location and orientation of the sensor, and which cell is 
being updated. Also, while the sensor reading M> may represent a continuous number 
indicating distance from the sensor, in general, each time the sensor is polled it will return 
an element from some set and M, will range over all elements of that set. 

The sensor model 1s usually independent of the current map and can be stored in 
tables. A further speed up of the process can be achieved if the logarithm of the above 


probability ratio is used. In this case the model uses: 


p(o| M, A M,) _ p(o| M;) , 7 p(o| M,) 
p(o| M,AM,) p(o| M,) p(o| M,) 


(5) 

In the logarithmic method the combining formula is changed from a multiplication 
to a simple addition. In this case the logarithmic result itself can be considered as weight 
of evidence of cell occupancy. Therefore, the need for only a single addition per cell 


allows for very rapid updating of the evidence grid map based on the newly acquired 


sensor data. 
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IV. FRONTIER-BASED EXPLORATION 


One of the goals of a robotic-based reconnaissance system is to reduce the amount 
of manpower required to reconnoiter an area. Any system worth building should be able 
to explore at least a limited area autonomously and in a fairly efficient manner. This 
means that the robots will have to be able to make use of the maps they will build as they 
move through their environment. Chapter III already discussed in great detail the way 
those maps might be represented, now it is time to discuss how a robot would use them 


to explore and map an area on its own. 


A. DEFINITION 


A robot exploring a new area will initially know nothing about that area except 
what it can detect in its immediate area with its own sensors. The limits of its sensors 
will form a boundary between known, explored space and unknown, unexplored space. 
Such a boundary is called a frontier. Within this boundary it is assumed that all obstacles 
are known and mapped. Thus, further exploration within this boundary would be futile. 
In order to maximize exploration in the least amount of time, the robot should move to the 
boundary between explored and unexplored space as soon as possible and use its sensors 
to expand the explored region. At the same time the act of expanding this known region 
will in turn create new boundaries or frontiers for the robot to explore. This is the central 


idea behind frontier-based exploration. This process 1s illustrated in Figure 8. [Ref. 22] 
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Figure 8. Example of frontier-based exploration. Robot begins by scanning immediate 
area, incorporates result into its map noting frontiers yet to be explored, moves to a 
frontier, and repeats the process. 
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Figure 8(a) shows a robot at startup in some unknown environment. The black 
circle represents the robot. The two large black rectangles represent obstacles in the area 
to be explored. The white space surrounding the robot is the area it can directly detect 
with its own sensors. The gray background represents the unexplored regions and the 
black dashed line represents the boundary between the known and unknown space. 

In Figure 8(a) the robot can detect the top edge of the first obstacle, but the 
obstacle blocks its view what may be behind the obstacle. Accordingly, the robot 
chooses a frontier and proceeds to that frontier in order to continue the process of 
exploration and map building. Figure 8(b) shows the robot in its new position now able 
to see behind the first obstacle. After scanning in this position the robot moves on to the 
position shown in Figure 8(c) where it can now detect the second obstacle. This process 
could continue indefinitely as long as there are unexplored frontiers for the robot to 


explore. 


B. FRONTIER DETECTION 


Before a robot can decide on a frontier towards which to proceed in order to 
continue exploration, it must first decide where the frontiers are within the region it has 
already explored and mapped. In order to detect these frontiers a process similar to edge 
detection and region extraction in computer vision (also known as machine vision) is used 
to find the boundary between mapped open space and unmapped unknown space [Ref. 


23). 
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Over the years many techniques have been developed in the area of computer 
vision to extract information from photographic images based on the pattern of changes in 
brightness in the picture [Ref 24]. Most of these techniques involve decomposing the 
image into a set of pixels, much like a grid. Each cell in the grid is given a value based on 
the brightness of the pixel that the cell represents from the original image. This grid is 
then searched for patterns that may indicate edges or patterns of interest. 

There are a variety of methods for picking out the patterns in the image depending 
on the information that is desired. Some techniques involve the use of a set of “masks” 
composed of a small (on the order of 3 X 3 or 4 X 4) pattern of cells of varying values 
that are successively laid across the original image. As the mask is moved across the 
image the cells of the mask are convolved with the cells of the image beneath the mask and 
the resulting matrix indicated the presence of edges in that portion of the original image. 
Other methods rely on simple histograms of the brightness values of the cells in the 
original image and attempt to use curve-fitting techniques to pick out edges of objects in 
the real world. 

Regardless of the methods used, these same types of techniques can be applied to 
detecting boundaries in frontier-based exploration. The same underlying grid format will 
be used as in computer vision, but in the case of frontier detection the values in the cells 
do not represent the brightness of a pixel. Rather, they represent the occupancy 


probability of a cell in the area the robot is exploring. 
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In general each cell in the area being examined for frontiers will be placed into one 
of three categories [Ref. 23}: 

e open: when the current occupancy probability of the cell is less than the prior 
probability 

e unknown: when the current occupancy probability of the cell is the same as the 
prior probability 

e occupied: when the current occupancy probability of the cell is greater than the prior 
probability 

A short explanation about the term “prior probability” is needed. When the 
evidence grid is first created it is necessary to initialize the cells in the grid to some value. 
Since at creation nothing in the grid has been explored it is logical to initialize all the cells 
to an occupancy probability value representing unknown, unexplored areas. All the 
implementations of frontier-based exploration discussed here [Ref. 22, 23, 25] initially set 
the cells’ values to 0.5. When the frontier-detection process is first done it is this initial 
(prior) occupancy probability to which the current occupancy probability will be 
compared. 

After a robot starts up or moves to new frontier it will make a sensor scan of the 
surrounding area. Based on the information returned from its sensors it will updated the 
occupancy probability value of any cell with direct range of its sensors. The robot will 
then use this updated information to perform frontier detection. Any open cell adjacent 
to an unknown cell will be labeled as a frontier edge cell. Adjacent edge cells are then 


grouped into frontier regions. Any frontier region above a certain minimum size (say 


ay 


roughly the size of the robot) will be considered an accessible frontier and marked as such. 


[Ref. 23] 


C. NAVIGATION 


Once the robot has scanned an area and updated all cells of the evidence grid 
within range, it must now safely and efficiently navigate to a new frontier in order to 


continue exploration and map building. 


1. Route Planning 


Navigation to a new frontier should involve a path that is completely within 
known space, therefore, all obstacles in that space should be known. Route planning 
involves choosing the best path (based on some criterion such as length of path, nearest 
approach to obstacle, etc.) through the known space to the frontier to be explored. This 
path will be based on the latest update of information concerning explored space. 

There are various algorithms such as depth-first, breadth-first, and A* search 
routines [Ref. 26] that attempt to search through a known map for safe and efficient 
navigation paths. However, a problem can arise if an obstacle (for example, another 
robot) moves into the chosen path since the last time information about the known space 
was updated. This may lead to the path the robot chooses to move to a new frontier 


being blocked. In that case, in order to avoid collision with the new obstacle and continue 
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navigating to the chosen frontier requires that the robot have some means of reactive 


avoidance. 


Ze Reactive Obstacle Avoidance 


Reactive obstacle avoidance entails some method of detecting and reacting to 
mobile obstacles that appear in the robot’s path that were not in the then-current map 
used by the route planning routine to plot a safe path for the robot. Mobile obstacles 
may include humans, other robots, or any of a number of other unpredictable phenomena 
that may exist in the robot’s world. For the most part, reactive obstacle avoidance 
involves the use of relatively short-range sensors (IR, contact, etc.) or long-range sensors 
(sonar, vision, etc.) scanning in the area immediately surround the robot as it moves. 

One important note about sensors and reactive obstacle avoidance is that the 
required scanning rate of the obstacle avoidance sensors is closely related to the expected 
travel rate of the robot and the anticipated characteristics of the area in which it is 
traveling. Obviously a quickly moving robot in an area where new obstacles appear 
frequently will need to scan the local area around it much more frequently than a slow 
moving robot in a well known, stable area. 

There are several different actions that a robot might take upon detecting a new 
obstacle along its planned path. One common method Is to use some sort of very low- 
level navigation routine that simply finds a path around the new obstacle and gets the 
robot back on the previous planned path as quickly as possible. This eliminates the need 


to call on the slower full route-planning algorithm. For small obstacles in the robot’s 
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planned path this is usually the method used. However, a problem can arise with this 
method when the new obstacle is so large that the low-level process cannot easily find a 
way for the robot to get around it. Normally, in this case the robot would stop and the 
full route-planning algorithm would be called on to find a new path to the robot’s 


destination based on the new information about obstacles in the robot’s path. 


3. Localization Error 


Every time the robot moves there is some slippage of the wheels that will cause 
the odometric encoders to incorrectly record the distance and direction the robot has 
traveled. Eventually, without some means of correction, the localization errors become so 
great that mapping and navigation become impossible. Methods of minimizing 
localization errors in mobile robots are the topic of much research [Ref. 20, 22, 27]. 

Figure 9 demonstrates the effects of robotic mapping with and without 
localization error correction methods while mapping a long, obstacle filled hallway. 

Figure 9(a) shows an evidence grid representation of the area to be mapped. Figure 9(b) 
shows a map developed by a frontier-based exploration system without the aid of any 
localization error minimization techniques. As the robot’s coordinates become uncertain 
the sensor return data begins to “drift.” Figure 9(c) shows the map which was developed 
using a continuous localization process that seeks to correct for dead reckoning errors that 


accumulate as the robot moves throughout the area to be explored. [Ref. 22] 
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D. NRL IMPLEMENTATION ON SINGLE NOMAD 200 ROBOT 


The full implementation of the frontier-based exploration code (for both single and 
two robot systems) as developed at NRL consists of over 60 separate C and C++ 
routines that are then compiled into a single process. Many of these routines handle the 
various ‘housekeeping’ functions of any large, complicated program (i.e. display, 
input/output, etc.) and do not bear directly on this thesis. Relevant routines and their use 
will be discussed below and portions of the code from these routines will be reproduced 
in the appendices. 

The latest version of the full code (as well as all previous versions) is available 
from Brian Yamauchi (yamauchi@aic.nrl.navy.mil) at NRL’s Navy Center for Applied 
Research in Artificial Intelligence (NCARAI). The NPS modified version of the code is 
available from Xiaoping Yun (yun@ece.nps.navy.mil) at the NPS Department of 


Electrical and Computer Engineering (ECE). 


1. System Overview 


There are a few major processes in the NRL code that bear directly on single 
robot, frontier-based exploration. The three key parts of their single-robot system that 
are relevant here are: the use of laser-limited sonar (LLS) for sensor scanning at new 
frontiers, the exploration routine used for the detection of new frontiers and the 
subsequent movement to and scanning of those frontiers, and the integration of the new 


scan with the robot’s current map. 


+H 


Ze Laser-Limited Sonar (LLS) 


There has been much study of the characteristics of the simple type of Polaroid 
sonar units found on the NOMAD 200, the NOMAD SCOUT, and many other research 
and commercial robots [Ref. 28]. The low cost, low weight, and low power consumption 
of these types of sonars has made them very popular among robot builders and designers, 
however, they do have their drawbacks. As noted in Chapter III sonar returns in the real 
world environment tend to include much noise and many extraneous or false returns. 
Many of these questionable sonar returns are caused by a phenomenon known as 
specular reflection. 

Specular reflection occurs when a sonar pulse hits a flat surface at an oblique angle 
and reflects away from the sensor instead of directly back to the sonar detector [Ref. 25]. 
When this happens there may be several different results depending upon the 
circumstances. Ifthe sonar pulse reflected off of the oblique surface encounters a flat 
reflective surface soon after, then the detector may still get a return, but the first object 
that the sonar pulse struck will appear to be further away than it is in actuality. If the 
sonar pulse continues on to the sonar’s maximum range without striking another object, 
then the nearby object may not be detected at all, but instead it will appear that there is a 
large open space surrounded by an unknown area. 

In reality, the possibility of not detecting a nearby obstacle is not as great as it 
first appears. With many sonars onboard the robot firing from several different angles, 


there is usually not much problem with getting some measurable level of sonar from 
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nearby objects and detecting them even if there are facing obliquely to some sensors. 
However, the problem of detecting “phantom” obstacles and false open spaces is still a 
problem that needs to be considered. Figure 10 provides an example of a normal sonar 
return and two of the possible results of a specularly reflected return. 

Figure 10(a) illustrates a normal sonar return with the sonar pulse represented by 
the ray emanating from the sonar, striking the nearby obstacle, and a measurable amount 
of energy returning to the detector. The head-on encounter of the sonar pulse with the 
flat face of the obstacle toward the sonar provides for a clear return path. In Figure 10(b), 
however, the oblique angle of the nearby obstacle reflects the sonar pulse away where it 
encounters the second obstacle. Ifa measurable amount of energy is returned from the 
second obstacle, then it may appear to the sensor that there 1s a “phantom” obstacle at 
the point shown in the figure. There may be a partial sonar return from the first obstacle 
as well, which could further confuse the sensor about the exact nature of nearby obstacles. 

Figure 10(c) demonstrates what may happen if there is no second obstacle within 
the maximum range of the sonar sensor after the pulse has been reflected from the first 
nearby obstacle. The sensor would receive either a very weak or non-existent return from 
the nearby object. If no return is received then it will appear that a large open space 
exists in the area shown, when in fact there is really no information known about that area 
at all. Since this false open region would most likely be surrounded by unknown space it 


would also have the effect of creating false frontiers for the robot to explore. 
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Figure 10. Examples of a normal sonar return, a specularly reflected sonar return that 
creates a phantom obstacle, and a specularly reflected pulse that generates a false open 


space. (After Ref. [25]) 
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In addition to the sonar sensors, the NOMAD 200 has a laser based range finding 
systems that does not suffer from these same type of specular errors. The researchers at 
NRL have taken advantage of this and created a technique known as laser-limited sonar 
(LLS). By using the readings from the laser rangefinder in combination with the readings 
from the sonar it 1s possible to eliminate most many false readings from walls and other 
large obstacles that cause the majority of specular reflections. If the laser returns a range 
to obstacle less than the sonar, then the evidence grid is updated as if the sonar had given 
the range indicated by the laser. [Ref. 23] 

The laser cannot be used exclusively for mapping because the laser rangefinder 
currently available on the NOMAD 200 only operates in a two-dimensional plane, while 
the sonar senses obstacles within a three-dimensional cone radiating out from the robot. 
Objects above or below the plane of the laser will be missed by the laser, but detected by 
the sonar. Figure 11 provides an example of how this might happen. In this figure the 
laser plane is above the obstacle and thus the laser rangefinding system never detects it, 
but the sonar cone emanating from sonar sensor does intersect the obstacle. This is a case 
of using two different sensor types that compliment one another. A three-dimensional 
rangefinder would be an alternative that would combine the best aspects of both sensors, 
but presently these type of systems are too large, expensive, and power consuming to be 


commonly used on mobile robots. [Ref. 23] 
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Figure 11. Example of two-dimensional laser rangefinder failing to detect an obstacle that 
is within the detection cone of the sonar sensor. (After Ref. [25]) 


3. Frontier-Based Exploration Routine 


The heart of the frontier-based exploration code is in the file agent.cc. It is in this 
operation that the frontier-detection, navigation, and exploration behaviors are described. 
This procedure also controls the laser-limited sonar scanning technique mentioned above 
and also takes care of integrating the newest scan with the current map via the method 
described below. The process begins by completing an initial sensor scan of the area 
upon startup and using the data obtained to construct an initial evidence grid map. After 
the initial map is created, a frontier detection subroutine is called to find and note nearby 


frontiers for further exploration. Once the frontier detection is complete the exploration 


49 


and navigation subroutines are called to choose the robot’s next destination and get it 


there safely. 


a. Frontier Detection Subroutine 


The method of frontier detection for this initial map and all subsequently 
generated maps involves using the edge detecting techniques described above on the most 
current evidence grid map that the robot has at that time. Mapped obstacles or edges 
separate frontier regions. The centroid (roughly the center of a non-symmetric region) of 
each frontier region is marked as the robot’s target destination for exploring that region. 

Figure 12 illustrates the different parts of the frontier detection process. 
Figure 12(a) demonstrates an evidence grid built by a robot 1n a hallway next to two open 
doors. Figure 12(b) shows the frontier edge segments detected in the evidence grid by the 
frontier detection process. Figure 12(c) shows the frontier regions that are greater than 
some threshold value (in this case roughly the size of the robot). The centroid of each of 
the frontier regions is marked with a crosshair and numeric label. The frontier regions 
labeled 0 and 1 represent the open doorways and the frontier region labeled 2 is the 


unexplored portion of the hallway. [Ref. 22] 


b. Exploration Subroutine 


Once the frontier regions have been found on the most current evidence 
grid map, the robot must decide which frontier to explore next. The path planner in the 


exploration code uses a relatively simple depth-first search on the evidence grid. It starts 
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at the robot’s current position and attempts to select the shortest obstacle-free path to 


the centroid of the frontier region chosen as it next destination. [Ref 23] 
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Figure 12. Example of frontier detection. From left to right: the evidence grid 
constructed, the frontier edge segments, and the frontier regions with the centroid of each 
region labeled. (From Ref. [22]) 


Cc. Navigation Subroutine 


Once the exploration subroutine has chosen a frontier to explore it is the 
goal of the navigation subroutine to get the robot to its intended destination in a safe and 
efficient manner. Using the path chosen by the depth-first search and a variety of 
reactive obstacle avoidance behaviors the navigation subroutine guides the robot. The 


navigation method used in the exploration code is sufficient to allow the robot to steer 
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around small obstacles in order to get back on the pre-selected path. If the robot becomes 
blocked or for some other reason cannot make any progress toward its destination, then 
after a certain amount of time, that location will be added to the list of inaccessible 
frontiers that the robot cannot reach. At that point the robot will conduct another sensor 
scan, update its current evidence grid map, and attempt to navigate to the next accessible, 


unknown frontier as chosen by the exploration subroutine. 


4. Integrating New Scan with Current Map 


The map integration routine uses the method described in Chapter III for fusing 
the new sensor data onto the current map. The frontier-based exploration process uses a 
modified version of the log-odds Moravec code described in [Ref. 17]. In the NRL code, 
each cell of the evidence grid is assigned a value from —127 (definitely empty) to +127 
(definitely full) for its occupancy value. 

When the sensor fusion procedure described earlier is used to combine the current 
map data and the new scan data, the result will be a new map that reflects the effects of 
the most current sensor data. Similar data between the current map and the new scan will 
tend to reinforce one another driving well-mapped cells toward a floor value of —127 or a 
ceiling value of +127. Likewise, if the data in the new scan conflicts with the data in the 
current map, occupancy values of those cells will be driven toward smaller absolute 
values with zero representing a cell whose occupancy is completely unknown. 

There is one important thing to note about coordinate systems used in the 


exploration process. When first initializing the robot, the user is asked to enter the 
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robot’s X, Y coordinates and an initial orientation angle. For single robot exploration this 
is not necessarily required, as the robot could assume that its starting position and 
Orientation are 0, 0 and 0 degrees and build a map around that point. However, for 
multiple robot exploration the robot knowing its starting position becomes very 
important as all maps sent to other robots are referenced to the shared global coordinate 


system. This will be discussed in more detail in the following chapter. 


EK. NPS IMPLEMENTATION ON SINGLE NOMAD SCOUT ROBOT 


In order to use the frontier-based exploration code developed for the NOMAD 
200 on the NOMAD SCOUT many modifications to the original NRL code are 
necessary. Throughout the original code there are many obvious, as well as hidden, 
dependencies and assumptions based around the sensor suite and mobility base of the 
NOMAD 200. Most of the modifications can be broken down into one of two 
categories: those changes due to the differences in the movement commands between the 
NOMAD 200 and the NOMAD SCOUT and those changes due to differences in the 


available sensors on the two platforms. 


I. Mobility Modifications 


As described in Chapter II, the NOMAD 200 has a three-wheel synchronous 
drive system with a turret that can rotate independently of the base, while the NOMAD 


SCOUT is a two-wheeled, two-degree of freedom differential drive robot which has no 


Ss 


turret. The NOMAD SCOUT was the first platform from Nomadic Technologies 
(manufacturer of all the NOMAD hardware and software products) that was built around 
such a mobility base. All their software prior to this had been designed for a three-wheel 
synchronous drive system with an independent turret. Early adopters of the NOMAD 
SCOUT, such as NPS, are using modified versions of the standard NDHE to interface 
with the new platform. Nomadic is currently developing a much more advanced version 
of the NDHE that will be more flexible in terms of working on multiple types of robots 
with varying mobility and sensor capabilities. 

Fortunately, Nomadic has developed a set of macros that accept differential drive 
commands for the NOMAD SCOUT and transforms them into equivalent synchronous 
drive commands that the software understands. These modified movement commands 
convert the decoupled translation and steering commands used by the NOMAD 200 into 
differential drive values required for the NOMAD SCOUT. So when the software is 
controlling NOMAD SCOUT it is actually modified synchronous drive commands that 
are being sent to the robot. To eliminate any software conflicts due to the lack of a turret 
on the NOMAD SCOUT, the conversion macros send a null (zero) value in place of any 


turret rotation commands to the robot. [Ref. 10] 


2 Exploration and Navigation Modifications 


The exploration and navigation modifications from the NOMAD 200 to the 
NOMAD SCOUT are much more extensive than the mobility modifications. The 


assumptions about the sensor suite of the robot designed into the original frontier-based 
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exploration code make for a large number of modifications to work on the NPS research 


platform. 


a. Elimination of Laser-Limited Sonar Dependency 


In the original frontier-based exploration process the laser-limited sonar 
technique described above is used to scan whenever the NOMAD 200 reaches a new 
frontier. Since the laser rangefinding system is fixed in place on the NOMAD 200 turret, 


this involves rotating the turret a complete 360° while the base of the robot remains in 


place. There is also an option to use only the sonar sensors to scan at new frontiers. If 
the sonars are being used as the only sensor, there is another option to rotate the turret 


through the 22.5° arc that separates the sonar sensors and take sonar readings at intervals 


along that rotation. 

In theory this gives the sonar sensors more opportunities to see obstacles 
from varying angles that might be less affected by specular reflection due to variations in 
the obstacle surface and what portion of the obstacle is struck by the sonar pulse. By 
modifying this sonar-only option to turn the entire robot instead of just the turret, it is 
possible to use this process on the NOMAD SCOUT in order to complete a sonar sweep 
upon reaching a new frontier. In addition, switching to the sonar-only option removes the 


dependency on a laser rangefinding system that the NOMAD SCOUT lacks. 


b. Compensation for Inability to Timestamp Sonar and Pose Data 


Originally it had been thought that once the laser-limited sonar dependency 


had been removed, that it might be possible to have the NOMAD SCOUT collect sonar 
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range data while on the move and eliminate the need to stop at frontier boundaries in order 
to collect new sensor readings. Taking sonar readings while moving can present a problem 
because sensor readings are not instantaneous and the robot’s position or the environment 
can change significantly between acquisition and processing of the sensor data, thus 
causing the collected data to be inaccurate [Ref. 12]. 

At the speeds both the NOMAD 200 and NOMAD SCOUT typically 
travel this does not cause any difficulty for the reactive obstacle avoidance routine, but it 
can cause problems for the accuracy of the mapping routine. On the NOMAD 200 it is 
possible to attach the robot’s pose information and a timestamp to every sensor reading 
so that data taken while the robot is moving can be correctly interpreted. The NOMAD 
SCOUT lacks this capability. In order to ensure that the sensor readings and pose 
information were as closely matched as possible it was necessary to break the sonar 
sweep at new frontiers (as described above) into small, individual movements. After each 
movement the robot was halted, the sonar readings were taken, and the sweep was 
continued. This has the effect of slowing down the overall mapping effort and the 


repeated small movements tended to increase the localization error. 


C. Specular Reflection Minimization and Side Effects 


In another one of the exploration code files (grid.h) it is possible to set the 
range at which sonar return data is considered “trustworthy.” Because the NOMAD 200 
has a laser rangefinder to confirm those sonar readings it is possible on the original code to 


leave this setting relatively high and disregard false readings. In the original code this 


56 


maximum sonar range is set at ten feet from the robot. Since the NOMAD 200 lacks a 
method of double-checking its sonar data this value is reduced to six feet. This also helps 
reduce errors due to specular reflection because it has been found that specular reflection 
errors are more prevalent at longer ranges [Ref. 28]. 

Unfortunately, there is an undesired side effect of reducing the 
“trustworthy” sonar range on the NOMAD SCOUT. With the decrease in range also 
comes a proportional decrease in the amount of new territory that is mapped whenever 
the robot reaches a new frontier. Mapping less area each time leads to an increase in the 
number of nee frontiers to which the robot must travel in any given area to be explored 
compared to the number of frontiers with a longer sonar range. Increased travel leads to a 
faster buildup of localization errors when only dead reckoning from the robot’s odometric 
encoders is used to determine the robot’s position in the global coordinate system. 
Neither the original NOMAD 200 nor the current NOMAD SCOUT versions of the 
frontier-based exploration system incorporate any sort of localization error minimization 
process. Later work on the NOMAD 200 has included work with a continuous 
localization process [Ref. 20, 22] and it is hoped that this method or one like it may also 


be used on the NOMAD SCOUT in the future. 


d., Reactive Obstacle Avoidance 


While navigating to a new frontier, the reactive obstacle avoidance 
subroutine of the original exploration code uses the infrared sensors as well as the sonar 


sensors on the NOMAD 200 to detect nearby objects. The NOMAD SCOUT lacks 


a) 


infrared sensors and it is necessary to remove any dependency on them and rely solely on 
the sonar sensors during the navigation and movement process. 

It is also necessary to make some relatively minor modifications to 
routines that take into account the robot’s size when determining if there is enough open 
space in a doorway or corridor for the robot to safely travel. The diameter of the 
NOMAD SCOUT is slightly less than that of the NOMAD 200 and thus it can move 


into a more constrained space. 
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V. MULTIPLE ROBOT INTEGRATION 


Even a very capable and very well equipped single reconnaissance robot will still 
be restricted in the amount of area that it will be able to cover in a given time by the limits 
of its mobility base and the range of its sensors. In addition, a single robot reconnaissance 
system is very vulnerable in that a single failure on the one platform can have eaOtrophic 
consequences on the ability of the system to perform its mission. By combining multiple 
robots together into a single integrated reconnaissance system, it is possible to have a 
greater area of coverage in a given time, quicker coverage of a given area, and continuous or 
overlapping coverage of high value target areas of interest. In addition, the use of multiple 
robots provides for a graceful degradation, rather than failure, of the system if individual 
robots fail to perform for some reason. In Chapter IV the mechanics of a possible single- 
robot exploration system were discussed. In this chapter the dynamics of using multiple 


numbers of such robots will be explored. 


A. CENTRALIZED VERSUS DISTRIBUTED CONTROL 


There are two differing philosophies concerning command and control of multiple 
robot systems. The centralized approach advocates some sort of supervisor or controller 
process that receives inputs from the individual robots and provides information and 
commands back to them. The distributed approach calls for processing sensor data at the 
local level and individual robots making autonomous “decisions” based on that 


information. Between these two methods there is a wide range of combinations and 


> 


permutations depending on the intent of the system designers and how they choose to 
implement the system. 

In the case of extremely centralized control there may be very little on-board 
“intelligence” on any of the individual robots and all commands of any sort (including 
motor and actuator commands) may have to come from the supervisor process. In this 
case the individual robots are little more than remote sensors on a mobility platform 
teleoperated by a central controller. The advantage of such a system is that each 
individual platform may be cheaper per unit. The main disadvantage is that highly 
centralized control leads to a single point of failure for the entire system and it will most 
likely also have a high bandwidth requirement if all raw sensor data and motor commands 
are required to be sent over the air [Ref. 29]. Another, less centralized, system may allow 
for centralized collection of processed sensor data from the individual robots which is 
then sent out to all the robots which then independently choose their own destinations 
for further exploration. Yet another system may call for the supervisor process to 
explicitly designate where individual robots will travel to and what tasks they will 
perform. 

In a fully distributed system each individual robot might operate completely 
autonomously of the rest of the system. More autonomy on individual robots can lead to 
increased complexity and unit cost per platform, but it also allows elimination of the 
single point of failure problem that plagues highly centralized systems. Autonomous 
operation does not preclude cooperative effort between robots, but without a central 


supervisor it can complicate the problem of coordination. Lack of coordination in a 
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distributed system may lead to decreased efficiency and possibly even counterproductive 
behavior on the part of individual robots in respect to the goals of the system. This will 


be discussed in more detail in Chapter VI. 


BE. SENSOR FUSION 


The evidence grid based mapping technique as described in Chapter III provides a 
good basis for the integration of sensor data from multiple, geographically separated 
robots. Sensor readings from different robots are fused in the same manner that sensor 
readings taken from a single robot at multiple locations are fused together. All that is 
required is that the sensor readings be referenced to a common global coordinate system in 
order for the sensor data from multiple locations to be properly correlated. 

The details of how and where the sensor fusion is done will vary depending on the 
organization of the rest of the system. In a centralized system the coordinator/controller 
might receive all the individual robot sensor readings, fuse the data into a new global map, 
and redistribute that information throughout the system. A more robust distributed 
system might allow for each individual robot to receive all the other robots sensor 
readings (or at least those nearby), perform the sensor fusion locally, and “‘decide” for 
itself where to explore next based on some given criteria. Other implementations might 
allow for limited local processing of sensor data at the individual robot level with the 


resulting details transmitted to a remote higher level supervisory process. 
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c. COOPERATIVE EFFORT 


In order to maximize the efficiency of a multiple robot system there has to be 
some degree of cooperative effort on the part of the individual robots. There are many 
possible degrees of cooperation as well as a multitude of methods and means of 
implementation. Both communication and coordination may be explicit or implicit with 


many varying combinations in-between. 


1. Explicit Communication and Coordination 


An explicit communication model allows for directed communications between 
each individual process and every other individual process as well as to and between any 
controlling or supervisory processes as well. While this model allows for a high degree of 
coordination and control, it can also become a communications nightmare very rapidly. 
The number of required links, L, for N separate processes in a fully interconnected 


system is given by: 


N-]I 


coy (1) 


i= 


Another variation of this communication model 1s to use a few, or even just one, 
common broadcast channel(s) and then to attach some form of addressing to each message 
sent. Each individual robot or process then listens to the common channel(s) for 
messages addressed specifically to it, ignoring all others. This provides much the same 
functionality of the totally interconnected model with much less communications 


complexity. 


62 


Explicit coordination involves the passing of directions or orders, as compared to 
simply information, from a process outside the individual robot in order to influence or 
direct the robot’s actions. It removes a degree of autonomy from the individual robots 
(which may no longer have a choice about their tasks and behaviors) and moves the 
decision-making ability to a higher level. Explicit coordination is usually associated with a 
hierarchical organization, but it can also be implemented on a peer to peer level where all 
the robots may be “equal,” but at least on a temporary basis one robot may be able to 
direct the actions of another [Ref. 30]. 

Explicit communication and coordination is exemplified by the process of a 
parking lot attendant directing cars into and out of the parking lot. The attendant is 
explicitly communicating with each of the vehicle drivers through a series of hand and arm 
gestures (often accompanied by verbal expressions). The attendant is also providing 
explicit coordination amongst all the vehicles and has a direct line-of-sight communication 


channel with each driver. 


Zz Implicit Communication and Coordination 


Implicit communication calls for processes not to pass information directly to 
another process, but to still convey information in some manner to interested parties. 
This may involve some sort of display or broadcast on the sender’s part, or the process 


or robot may have some sort of noticeable behavior that an outside observer can interpret 


Piet. 5 |i], 
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In general implicit communication has lower direct communication requirements 
from robot-to-robot or robot-to-supervisory process. However, there is a corresponding 
increase in the requirement for an individual robot to be able to interpret other robots’ 
behaviors or displays and extract useable information them. This requirement may have a 
great effect on the unit cost per robot depending on the complexity of the information 
implicitly passed from robot-to-robot or robot-to-supervisory process. 

Implicit coordination involves a single robot interpreting the actions and behaviors 
of other robots in the environment around it and taking individual action in accordance 
with the general goals of the system. This calls for a higher degree of autonomy on the 
part of the individual robot in order for it to know when to do the “right” thing at the 
“right” time. Implicit coordination is generally associated with a peer-to-peer 
organization where all robots are “equal,” but it can also be implemented in a hierarchical 
structure with “lower” robots taking appropriate cues from the behavior of “supervisor” 
robots and vice-versa [Ref. 32]. 

Implicit communication and coordination is perhaps best exemplified by the 
process of an audience leaving a theater at the end of a movie or play. Generally, there is 
no overarching supervisor directing people into line and out of the theater and people do 
not explicitly announce their intentions to move into line to those around them. Instead, 
each person observers the actions of those around him/her and making a decision on when 
and where to move based on their actions and behaviors and following the general goal of 


leaving the theater. 
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D. NRL IMPLEMENTATION ON TWO NOMAD 200 ROBOTS 


The interprocess communications routines used in the frontier-based exploration 
code were developed by Bill Adams (adams@aic.nrl.navy.mil) at NRL’s NCARAI 
facility. The NPS modified versions of these routines are available from Xiaoping Yun 
(yun@ece.nps.navy.mil) of the NPS ECE Department as part of the NOMAD SCOUT 
modified frontier-based exploration code. Relevant routines and their use will be 


discussed below and portions of the code will be reproduced in the appendices. 


1. System Overview 


There are a couple of processes in the NRL code that bear directly on multiple 
robot, frontier-based exploration. The two key parts of their two-robot system that are 
relevant here are the communications process itself and the process of a robot integrating 


another robot’s map with its own. 


2 Communication Process 


In the NRL code for two-robot, frontier-based exploration information about the 
world is shared, but each robot maintains its own map and makes its own decisions about 
where to navigate [Ref. 25]. There is no higher level supervisory process directing the 
individual robots or coordinating their actions. Normally this would be thought of as 


implicit communication and coordination. However, the system has to make use of an 
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explicit communication architecture to emulate the implicit communication process due to 
the limitations of the available networking protocols. 

Even thought conceptually the process that is modeled is a peer-to-peer 
relationship the limitations of using existing TCP/IP networking tools preclude the 
system actually implementing this model. Instead, the original code simulates a peer-to- 
peer relationship using a standard client-server model. In the two-robot code the first 
robot is always designated as the server while the second robot is always the client. 

Also, even though the model of the original code implies implicit communication, 
messages concerning new map availability are actually passed explicitly from robot to 
robot. It is also important to understand that in both the NOMAD 200 and the 
NOMAD SCOUT implementations that there is no controlling process actually running 
on the robots themselves other than low level motor controls in the robot’s firmware. 
The controlling process of the robot is running on a remote UNIX workstation and all 
“communications” between robots is actually communication amongst the remote 
controlling processes. Also each controlling process is a client to the Nserver process. 
Figure 13 provides an illustration of how this is all connected together for a two-robot 
system. 

As shown in Figure 13, all three processes, the Nserver and the two mobile robot 
processes, are running on the same UNIX workstation. In reality these three can all be on 
the same or separate workstations or any combination in-between as long as there is a 
shared memory location to which each robot process can write the map it will share with 


the other robot processes. 
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Figure 13. Illustration of the communication process used for the two-robot frontier- 
based exploration system. 


During the exploration process whenever the individual robot completes a sensor 
sweep at a new frontier, its controlling process writes the result of that local scan into the 
shared memory location. This file (locall.eg or local2.eg depending on which robot 


process that creates it) is an evidence grid representation of the most current local map of 
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the area surrounding the robot of the controlling process. In the original representation 
this file is not the full global map maintained by each robot process. 

After the robot process has finished writing the updated local map to the shared 
memory location it sends a message to the other robot process that there is a new local 
map available. This is a case where an explicit communication is used to simulate what 
would normally be a broadcast if allowed for by the network protocols being used. 
Instead of the updated map data itself being sent, there 1s a directed message to the other 
robot process transmitted. 

After the update message has been sent to the other process the controlling 
process checks to see if it has received its own message from the other process indicating 
that a new local map file is available. If there is one available it reads it from the shared 
memory location and proceeds to integrate that new remote local map into its current 
global map. Using this method writing new maps, sending messages to other robot 
processes, and checking for remote local maps to integrate is only done after a new sensor 
sweep has been completed at a new frontier. 

One of the drawbacks to this method is that it is possible for a robot process to 
miss a remote map update from another robot process. This can happen if a robot 
process is directing its associated robot on a long traversal from one frontier to a new one 
to be explored. It 1s possible for the other robot process to explore a frontier, write an 
updated map, move its robot to a new frontier, explore the new frontier, and overwrite 
the previous update all before the other robot reaches a new frontier and completes its 


exploration, writing, and remote map reading routines. 
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The original information is lost because the local map file that is written to the 
shared memory location only covers the immediate area around the robot. Once the robot 
moves to a new frontier and a new local map file is written it is very unlikely that there 
will be any map area overlap between the new map and the previous one. This can lead 
to a robot process making decisions on where next to explore based on incomplete 


information as to where other robots have already explored. 


3: Integration of Foreign Maps 


The integration of a foreign or remote local map uses the same methods described 
previously in Chapters III and IV for fusing new sensor data onto the current map. The 
sensor data from a remote map is fused with the robot process’s global map in the same 
manner as if the process’s associated robot had collected the same data itself. The 
important thing to note is that the remote local map must also have attached to it the 
global X and Y coordinates where the data was collected so that it may be registered 


correctly during sensor fusion with the global map. 


E. NPS IMPLEMENTATION ON FOUR NOMAD SCOUT ROBOTS 


In order to use the communications routines developed for the two-robot 
NOMAD 200 frontier-based exploration code on a greater number of NOMAD SCOUT 
robots a few modifications to the original NRL are necessary. None of the changes are 


actually specific to the NOMAD SCOUT so the modifications made here will work just 
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as well on an increased population of NOMAD 200 robots. It is hoped that other 
researchers with larger numbers of NOMAD 200 robots will be able to make use of the 
modified code developed at NPS. The modifications can be broken down into two parts: 
extending the client-server architecture to mange more than two robot processes and 


transmitting the global map vice the local map from the server robot process. 


it: Extended Client — Server Model 


There are a variety of possible different approaches to extending the original 
code’s client-server model for two robots to a client-server model for greater than two 
robots. In order to maintain the fully interconnected architecture of the original NRL code 
it would be necessary for individual robot processes to function as both clients and 
servers depending on the circumstances. An example of how this might work for four 
robot processes is shown in Figure 14. A process being both server and client 
simultaneously is not without precedent as all the robot processes are also clients to the 
Nserver and the first robot process 1s also a server to the second robot process in the 
original model. 

However, as seen in Figure 14, the number of interconnections increases rapidly as 
the robot process population grows. It becomes apparent that this is an unnecessarily 
complicated and unwieldy implementation for a large number of robot processes and an 
alternative method is used that while not fully interconnected between robot processes, 


still provides suitable connectivity for the transmission of remote map file information. 
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Figure 14. Illustration of a fully interconnected communications architecture for four 
robot processes using a client-server model. 


Instead a single robot process is used as a server and all the other robot processes 
are clients to that single server process. This model is shown in Figure 15. As in the 
original NRL code the first robot process acts as the server. Also, it should be noted that 


all the robot processes remain clients of the Nserver program. This approach eliminates 
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the full interconnection of the original NRL model, but greatly simplifies the modification 
of the code to work for larger numbers of robots. However, without the direct connection 
from client process to client process there is a need for another way to transfer remote 
local map information between client robot processes. The solution to this problem is 


discussed in the next section. 
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Figure 15. Illustration of the communications architecture implemented for four robot 
processes using a client-server model and retaining the single robot process as server. 


In this model each of the client processes 1s 1n effect part of a two-robot system, 
itself and the server process. Only the server process “sees” all the individual client robot 
processes. However, there is still a good deal of robustness to the system. If one of the 
client process robots fails the rest of the system will continue to function in the same 
manner with the smaller robot population. If the server process fails there will be no 
more sharing of map data, but each robot process will continue to explore individually 


until there are no more frontiers remaining. 


fe 


De Transmission of Global vice Local Maps from Server 


In the original NRL code whenever a robot process writes a map file to the shared 
memory location it is a local map file of just the immediate area surround the associated 
robot of that process and showing sensor data from the latest sensor sweep only. It is 
possible to modify the code so that instead of just writing the local map data, that the 
global map maintained by that process is written to the map file. This global map file 
incorporates all the separate local sensor sweeps that that process has made up to that 
time as well as any remote map files that it has fused into its global map. 

Modifying the code in this manner makes it possible for each robot to write the 
entire evidence grid representation of its global map to the shared memory location 
(globalx.eg where x is the number of the robot process). In this manner the server 
process incorporates the individual global maps of all the individual client processes into 
its own global map. When the client processes read the globall.eg remote map file written 
by the server process they then indirectly incorporate all the results of the mapping done 
by the other client processes. 

This also solves the problem of individual sensor sweeps being “lost” due to a 
short move, explore, write cycle causing the file to be overwritten. In the case of wnting 
the entire global map the new global map will incorporate the previous sensor sweeps as 
well. However, having all the separate robots write their individual global map files has 


an unexpected drawback as well. 


When only the local sensor sweep information is propagated from robot process 
to robot process then “bad” mapping data from one robot (due to sensor errors, location 
errors, etc.) may be overwritten when another robot happens to scan the same area. Also 
in this manner temporary obstacles (people, other robots, etc.) are steadily eliminated or 
their positions updated on the global map that each robot process maintains. By having 
each robot send its entire global map an unwanted feedback loop for the reinforcement of 
“bad” data can occur. 

If a client robot scans an area that happened to have a temporary obstacle or is 
very “noisy” due to specular reflection off of objects in that area, the results of that local 
scan will be incorporated into its own individual global map. Now when that global map 
is read by the server process it will fuse that data with its global map and write its 
updated global map to the shared memory location for all the client processes to read. 
After they read it and update their global maps, the next time they write their global maps 
for the server process they will also show the same “noisy” or temporary obstacle sensor 
data which will reinforce the previous data on the server process global map. The server 
process will in turn write this updated global map for the client processes to read and the 
feedback of incorrect or out-of-date sensor data will continue. This is not a desired 
situation. 

In the final version of the NPS modified code only the server process writes its 
global map for the client processes to read. The client processes write out only the 
results of their local sensor sweeps for the server process to read. In this implementation 


temporary obstacle data or “noisy” sensor data will be propagated through the system 
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once when the server robot incorporates it into its global map, but it will not be reinforced 
by having that same sensor data sent back to it from the client processes. The tradeoff to 
this approach is that once again the server process may miss a client process local map if 


the server process 1s busy controlling its robot during a long movement between frontiers. 
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VI. RESULTS 


Despite equipment delivery delays and the need for extensive software 
development, substantial initial testing of the NOMAD SCOUT multiple robot frontier- 
based exploration system at NPS was possible in the limited time available for research. 
Besides the results presented here, the major product of this research was a demonstrable 
robotic exploration system that will serve as a testbed for future projects involving both 


single and multiple robots. Presented here are the preliminary findings to date. 


A. SINGLE ROBOT MAPPING EFFORT 


Single robot mapping of a given area provided a baseline against which multiple 
robot mapping efforts were compared as well as ensured that the basic frontier-based 
exploration code and evidence grid map making routine functioned properly in 
conjunction with the NOMAD SCOUT robot. Early tests were also used to determine 
the best combination of map grid resolution, “trustworthy” sonar range, and given area to 
be explored that would yield optimal results for a single robot. The best combinations of 
these variables were then used for each individual robot in multiple robot mapping 


experiments. 


1. Single Robot Test Conditions 


The test area for all single and multiple robot trials was an approximately 37 by 


37 foot research room with two large test benches that defined three major corridors in the 
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space. An additional test bench along one wall further constricted one of the corridors. 
Figure 16 is a simple illustration of the area with annotations for the various starting 
positions used in the trials. The center of the room was defined to be the origin of the 
coordinate system used in the map produced during the robot mapping trials. This origin 
is marked as position zero in the illustration below. The various corridors are referred to 
as top, middle, and bottom as labeled in the sketch of the room. Note the windows 
stretching along one wall, the metal cabinets, and the large number of doors. These were 
geographical features that proved especially challenging during efforts to map the area due 
to specular reflection effects. 

Shown in Figures 17, 18, and 19 are a series of pictures taken of the test area in 
order to provide the reader with a better perspective of the environment. Note the large 
open spaces under the test benches. Because the lower portions of these benches were so 
close to the ground the sonar sensors often failed to detect them. It was necessary to fill 
in some of the space under the benches in order to enhance their sonar image before any 
worthwhile results were possible. 

Figure 17 shows the top corridor of the test environment. The metal desks and 
windows in this area proved particularly difficult to accurately map. Figure 18 shows the 
middle corridor of the test environment. This corridor runs down the center of the test 
environment and the midpoint of this corridor served as the origin of the coordinate 
system used in all the mapping trials. Again, the windows at the end of this corridor 


caused difficulties in the mapping trials. Figure 19 shows the bottom corridor of the test 


78 


environment. The smooth-surfaced doors and the metal cabinet in this corridor were the 


major sources of mapping errors. 
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Figure 16. Simple illustration of test environment for single and multiple robot trials 
showing Starting positions and significant geographical features. 
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17. Top corridor of test environment. 
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Figure 19. Bottom corridor of test environment. 


Zs Experimental Variables 


After the basic robotic exploration and mapping system was functional it was 
decided to concentrate on examining a few easily-manipulated variables in order to 


attempt to optimize the system for the given test area. 


a. Given Area to be Mapped 


The first thought was to minimize the area the robot would be expected to 
map. This variable is set in the file grid.h and sets the size of the room the robot will 
map. Minimizing this would seem to ensure the finest detail possible for the given 
evidence grid resolution (as described below). It proved not to be practical to set it 


exactly to the actual size of the test area. In a perfect world the robot could have been 
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instructed to map a 37 by 37 foot room and all would be well. However, as odometry 
errors began to accumulate during a test run, the robot became confused near the edges of 
the room when its now-inaccurate odometric encoders indicated that it was outside the 
boundaries of the expected area. 

In addition, specular reflection errors off of objects near the boundaries 
(especially the windows) caused false sonar returns from outside the boundaries set for 
the room. This caused additional errors. To alleviate these difficulties a 3.5 foot safety 
margin was added on each side of the room boundaries, resulting in a 44 by 44 foot area 
that the robot expected to map. This reduced the overall resolution slightly, but resulted 


in more consistently successful mapping efforts. 


b. Evidence Grid Resolution 


Also in the file grid.h the evidence grid resolution is set. In order for the 
evidence grid method to correctly fuse two different grids the grids must be symmetrical 
and a power of two. Varying resolutions from 64 by 64 cells to 512 by 512 cells were 
tested. 

The initial testing with a setting of 512 by 512 cells resulted in very noisy 
sonar data and many small frontiers. These small frontiers were often found to be 
grouped around one large object, especially one with many projections such as a chair or 
table. It was hoped that setting a coarser resolution would result in quicker mapping of 


large areas and less noise from the arms or legs of chairs and tables. It soon became 
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evident that using a very coarse resolution, such as 64 by 64 cells, did result in a shorter 
mapping trial, but not for the desired reasons. 

With the room size mentioned above of 44 by 44 feet and using 64 by 64 
cells in the evidence grid, each cell was about 68 in? or 8.25 inches on a side. This is a 
rather large size for a cell compared to the size of objects in the test environment. As 
expected, the noisy sonar returns were blurred into fewer cells, but the unfortunate side 
effect was that now large cells that were only partially filled were marked as completely 
filled, whereas with finer detail these areas would have been resolved into open space. 
With the coarse detail setting the robot soon marked all possible paths as blocked by 
obstacles when in fact there was still many open paths for it to travel. This is illustrated 
in Figure 20. Here the resolution was set at 64 by 64 cells and the robot was started at 
position zero in the center of the room. Even though the corridor was open, noisy returns 
were still blurred together to the point where the robot determined that is was completely 
blocked. 

Numerous trial-and-error investigations led to the choice of 256 by 256 
cells for the grid resolution 1n conjunction with the “trustworthy” sonar range discussed 
below. Again using the room size of 44 by 44 feet, but now with 65536 cells, each cell in 
the evidence grid was approximately 4.25 in? or less than 2.1 inches on a side. This was 
the best compromise found between reducing noisy data and having fine enough detail to 
navigate the robot and map properly. It is important to note that these results were 
specific to the environment the mapping tests were conducted in and will most likely be 


quite different in dissimilar circumstances. 
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Figure 20. Illustration of robot exploring corridor using coarse discrimination (64 by 64 


cells). Large individual cell size causes false determination that ends of corridor are 
blocked. 
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ued. Illustration of robot exploring corridor us 
(64 by 64 cells). Large individual cell size causes false determ 
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é. “Trustworthy” Sonar Range 


The final option that was manipulated in the grid. h file was the 
MAX SONAR_RANGE variable. This variable sets the “trusted” range for sonar sensor 
readings. Readings indicating distances further away than this setting will be disregarded 
for the purposes of map building and exploration. As was mentioned earlier, setting this 
to a lower value than the 10 foot range used with the NOMAD 200 seemed to be the best 
way to reduce the problem of specular reflection. Also, as mentioned previously, there 
was a penalty in setting this too low in the increased amount of travel the robot would be 
required to do and the subsequent increase in localization error. 

After many trials it was found that a six foot range was adequate to reduce 
many (but not all) specular reflection effects and did not seem to compromise the robot’s 
localization capability to any great degree. However, if an additional localization method 
is added to the NOMAD SCOUT platform in the future it is recommended that this range 
be further reduced in order to further mitigate specular reflection problems. Figure 21 
shows a sequence of maps created during a NOMAD SCOUT mapping sequence with 
the MAX SONAR RANGE set to 10 feet and a grid resolution of 256 by 256 cells. The 
robot was started at position zero in the center of the test area. Note the numerous and 
extensive specular reflection effects from the areas near the benches and windows. These 
false returns created numerous small, false frontiers that the robot attempted to explore, 


but could not reach. 
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Figure 21. Illustration of false sonar returns and subsequent poor mapping results due to 
extensive specular reflection when using 10 foot sonar range. 
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Figure 21 continued. Illustration of false sonar returns and subsequent poor mapping 
results due to extensive specular reflection when using 10 foot sonar range. 


3, Trial Runs and Results 


One thing is immediately noticeable from the many trial runs conducted with one 


robot in the initial research: as currently implemented no single robot alone will be able to 


map the entire test area. On average, after 20-25 minutes of travel the odometry errors 
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become so large that further mapping efforts are actually counterproductive. Continued 
mapping at that point, with localization so badly compromised, will most likely begin to 
overwrite previously accurate areas of the evidence grid map with inaccurate data. This 
was seen many times in longer trials. In the current implementation there is not enough 
time before odometry error becomes fatal to the exploration and mapping effort for the 
robot to cover the entire space. Thus the need for multiple robot exploration and 
mapping is evident. 

Figure 22 illustrates a typical trial run with the “standard” settings of a 256 by 
256 cell evidence grid resolution and a maximum trusted sonar range of six feet. This run 
was started from position zero in the center of the test area. Mapping efforts continued 
well for about the first 15 minutes. After that time, localization errors began to interfere 
with the robot’s ability to navigate to new frontiers. Localization continued to get 
steadily worse, especially rotational tracking. By the 21“ minute of the experiment the 
robot was actually travelling in the opposite direction than it indicated that it was moving. 
This seemed to be a common trend among many of the trials. The small movements that 
the robot makes during sonar sensor sweeps at new frontiers as well as the many small 
turning motions the robot makes as it travels seem to affect the rotational localization 


much more quickly and much more detrimentally than the translational localization. 
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Figure 22. Illustration of fatal localization error beginning about 15 minutes into the 
mapping and exploration phase. 
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Figure 22 continued. Illustration of fatal localization error beginning about 1) minutes 
into the mapping and exploration phase. 


However, other trials showed that localization errors could also cancel each other 
out and allow longer periods of mapping. These maps will be distorted compared to the 
actual “ground truth” of the area mapped, but they will still have recognizable, albeit 
distorted, geographical features such as corridors, corners, etc. Figure 23 is an example of 


such a trial. This run was conducted under the standard conditions with the robot 


9] 


initially started at position one. Between the nine and 19 minute point in the trial the 
robot was stuck in a small area between the two benches trying to explore many small, 


inaccessible frontiers. By the time it “broke free” its odometry was obviously distorted, 


but it was possible to still recognize map features produced for another 12-14 minutes. 
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Figure 23. Illustration of robot getting temporarily trapped in a small area, breaking free, 
and then continuing to produce a recognizable, although distorted, map for several 
minutes longer. 


2 





















ETE PS eT ae zeranuscrewenccgs ee rere eRe SED CeN Tye ama ae eer ey ara Ty OS ee ET aE Te TINE SSE. Tee TU eT SUE Ue SC el 
Pesan glue ead a isu iE ba Lua ee Hh He IN es renee ies ce ieee Ae Tirentw Oe me eT NeLe SUE AL Quoin Gms Owulig BEM e@cellem gli gueleiGwnlioeM elim PHEIYTUT ITU ee ee 
cele aen ub sh: balact ate peas eet ead Rein eres Pecraie pita iva= “sin: rgiapeng neues 0h Fs pavanar ey aires Gate PI Tet Pati EE br) 
* 
pat 18 180: Matewnaise on! Tomer ssi! 




















apeenge 














ge eae 





































V6) Od 1 81s 108, boy 3 xi : 3 SVE Taio Pere Te at es pied re top facia Titi poe ee el F108 “are 
irenenaidlensenisieen suns eee Teg mbnum ie sree Raat iiguees PE eH ULES ed ace id eae 
ee ee de ee 7 er fete |) i ed 

reberri Bech eS nicior aS . i bene e i im o ws Mnnd Tet thofn # Si LBW fe Bale Ll ut ies ay ‘eases ot Our f-0.2 S-N ELI 8 £8 thay & 21 a. 0k. 

Abies RUB eh. Tiamoaaele gol Seige = meee me " Tei EY) a oes Sttnenieneia: eer er Air Gen 8 11m GaAs Ose ae! 2s: orale 

ee ee i er) - = Tea ee Pee ee ah = ~ — 

oiTERIR Sor abs ; ate i. eter iseragaat TP wtiy art endive ating tei gerg: Gide dates reared TORT EES TS ere ite to Merete ninlagnea ten sae telebw-rpdar atren t 

Eh Bich el et feted indi 2 ge’ aoe ed bad b LL RILL Ma Hiupdneaigenes 1 Mp Lgt Or ercp srg eas ni ee de ferenu HTH BE Ab cane 

(eau wt ioe peineie su mek 2! ra Oa es pays eee a ielencare SRL eT SIS TSI IN EST Ey a ri v=. wr 
aie Ht uérwiin: Wenianee ion eHr erie iret 1 





ii er Hob peter ese 
SU ECR 
ig re ary fehe G 1 r 
apenas y 
tae aie i e i- Lee 
rrr rrr a 
ns ersietag 


premtuestct: 
ae 


SITES es ee 

bo 
stages Es & CST CT a aT 
aa estan ewer ee Temecdiermes ter ovTON Seger OiiQitntemse®<i0itiey oa iE O10 












edn aotce. Db dined git 2 itt ee Se pape ee es bt 
eae ae soem 


wee z.fumntidimidigue Ti rece ee eee et a Searels ‘rOriegs 101g 1818 
[aan Loe sla at ETO a rir t soe pe Oni) Cake st el 
eee esipeantnet -: es ee ans a a — 
eee te Tee atees en a ‘ : ; : at { uae udsauiaud oes 


tod 1-0 46 OTS of 108.. 
— 























ewer eee ane 

ot reed Wirt Fo ba ee 
COLL atM Lg EEN aime Sy jAfie nu biies: SA eee dpereetebels oan ibindrsba ewe 
35 cea = ee rere Bart Wate 


ieee ornare eee - 
ee ee eee a UH rE T ME or Gap Ler hs ew 1 6 ry Oi GAT Or gg OAR Less Bh ok eevee my etait emuetea rome 


EcrEEisy rare Srinath rm 






ee ee ee 


x 
e 
———o ef. 
PSH HOT EHeTe Oey ¥ 
carte neraneeeepere ene ras oe 


FIST res ce enh Bp —? rniee bor ied abaceded eek daha peer Een int 
uid aaEaT 3 bidebts ne bette ea ae 12 thai. 8 te ys 
ajid Ot gees: wait Ne tt ee, 


ge teas) 218 et --t 


ae 
Sy Uanoignetiig eae 
eee 


OO 5 remiss ees tolaees It; 2 
. f = 





% 
ee Bae a ienpceesteesmesninveees 
SET Te Te 
Gas see 




















gt Sea stnenteine haa 


e r ob ays sa ry 399 F 85bs 
ear 
i emg 2 
eles br R el 3. dis 8 Mi «de J A 
[sesso oi eect reoeee reer 
ec 


aa 
Tigntnainiinicn gianna 


Tisil ha Qe soi 











feraee zi 
Beeet 


a 
ee 


ee armen es ate a ee ee 
ra i ritthictis eras. Welt ceteined noite eee 


A 
y Bt 


guess aie % 





nent 
echrerin rr 





ape i 


s 
sooeasen so ee 
mer agi eee RSE F mlepents lene wel ] 9 Mi utes T 
rs. ater ET EE A Baa n Seer eee eee eeaneed apa tee Pd tae beets te et : ’ 
sana aiuto tos Re Tee te eee Se lielee ieielceisemelieierit gig: sauiniroriga in 1G ance fuinsie gen eieseigisnig i SESS eae Es 


— 


STE nucewe 
aii: Baten ant teacton BEE jes eget 



























TTR TET ETT ne Ce: CTCL Ts ST oe TOIT OMe: cetse ure nage estat Ours eter eT POST e | ETERS eo eo ee oT Snes Teter gCOlrEoRaS 
9 10011Ge@8l ee soctiny spensenceare eee: meuscegytizentie allt simeuaiuaniiaua aie es eicea sie tect. oe Te @ertGiig Gri @NLOs IGT Ou erel satu TIe e 


tee esr 






a 1) ups 
mivearrnsnict ah 


















ott: bis Biel icrarin en 3 ere sreseses! eer en: RIE TTRIRee eRe ren 
ae Rikon auscuuUnObeiueliubol GUALkGs GLeDLasuSie A A Rl rie wri 
jor 13— SS ae a ee Peers ts a st STRCCECInE TELee ie ie TInt apearry irene io : ee ee 
iy: Sor Stes it ree es rage ae ly ihe 
SS 
es = > 






— na 

G0 110 118 eT pr esreas 00 rSr STRATE 
carter neem ane 

nar ce iss rs 


rhe tah be 2 










iouibagra enigneies aT 


BX 

caPeet ea ats 

1.8 Se i 
cieperorierr retin PAG], 


shlgnerincenecumatiiermegnonen bet ora meal | 


Jap fied 






: t SEASON : Pol 
sein, ae ae aay 
we . 





Aiden rg 825 4.5. 4d. 


inneneny 


Saree 
ribet 
Bios 





ee 
ena TS kOe 08s Se ta ST Te 







ee 
cuca ut HH, 
sas 


Sreesereerrer str 
a naw ts Lut. & Ot patna 1.2.9 tes Si. ft 
Pacesiepa tag ¥ Soares ee a a 










nteee "Fa, 


A lh 

spent, e be Lie "tn =. 
eee aera oe, 

Stee os ee 


z EUR RaL aE bie Re eerier ry 


1 aed a 






Figure 23 continued. Illustration of robot getting temporarily trapped in a small area, 
breaking free, and then continuing to produce a recognizable, although distorted, map for 
several minutes longer. 


All single robot trials continued to point toward the need for multiple robots 
acting simultaneously in order to map the test area. Under the current implementation a 
single robot cannot map the area before localization errors render it incapable of further 


exploration and mapping. 
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B. MULTIPLE ROBOT MAPPING EFFORT 


Multiple robot mapping efforts provide a number of mixed results. In some 
circumstances the use of multiple robots dramatically decreases the amount of time 
required to map a given area compared to a single robot mapping the same area. In fact in 
some cases multiple robots were able to map an area that the single robot could not 
complete due to buildup in localization errors. However, under other circumstances 
multiple robot mapping can be less efficient than expected and actually worse than single 
robot efforts. There also appear to be some issues with the effects of controlling many 


robots simultaneously on network performance and reliability. 


1. Multiple Robot Test Conditions 


The test area for all multiple robot trials was the same as for the single robot trials, 
the 37 by 37 foot research room described previously. All robot processes, as well as the 
Nserver program, were run on the same Sparc 20 workstation used for the single robot 
trial and the robot processes controlled their respective robots via the same wireless 
Ethernet connection. Using the same workstation to run all the robot processes 
simplified the sharing of map data between the client and server processes, but did lead to 
an overall slowdown in the speed at which the individual processes ran as more robot 
processes were added. For all the multiple robot trials each individual robot was similarly 
configured with an evidence grid resolution of 256 by 256 cells and a trusted sonar range 


of six feet. 
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2. Trial Runs and Results 


One major finding from the multiple robot trials was that there was not quite the 
consistency or quality of performance improvement that had been expected. In a perfect 
implementation if a single robot can map_X area in Y time, then N robots should be able to 
map.X area in Y/N time (or conversely map N*X area in Y time). While such extreme levels 
of performance improvement were not expected at this point in the research, it was 
expected that there would be somewhat more improvement and more consistency in 


improvement than was seen. This will be discussed further below. 


Be Beneficial Effects 


Some multiple robot trial runs did show significant improvement in mapping 
efforts over single robot trials. This was especially true when the robots started in 
widely varying geographical positions in the test environment with well-known initial 
starting coordinates. In these cases each robot mapped its local area in the same manner 
as in the single robot trials and the server robot process consolidated the map data. Figure 
24 illustrates this process for an average two-robot trial. 

In the trial shown in Figure 24 one robot was started at position zero and the 
other robot was started at position five. With the two robots separated by an obstacle (in 
this case one of the benches) they explored their general area without interference from 
each other. One important thing to note about this trial is that the robot in the middle 


corridor suffered networking problems after approximately 20 minutes. It stopped 
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mapping, but the robot in the top corridor continued mapping. This illustrates the 
benefits of a distributed system without reliance on a central controller to operate. 

As the still functioning robot continued to map the top corridor localization errors 
soon began to degrade its navigation capabilities after about 23 minutes. At the 30 minute 
point in the trial further mapping efforts are futile. Note the specular reflection effects on 
the robot in the middle corridor near the windows and along the benches in the middle 
corridor. 

Figure 25 illustrates a representative three-robot trial with the robots starting in 
widely sepeeten positions. In this case the robot were started at positions two, eight, 
and nine. Again, each robot began to explore its local area without interference and the 
server robot process collected the local sensor data from each client robot process, fused 
the data, and distributed a new global map to all the robots in the system. 

For the 20 minutes that this experiment ran localization for each individual robot 
was maintained relatively well. The combination of the three robots managed to explore 
and accurately map more of the test area than a single robot would have been able to in 
the same amount of time. More importantly, even if a single robot could manage to 
navigate through the same amount of area that the three robots covered, its mapping 
accuracy would be much worse than the three-robot system. The longer time required for 
a Single robot to cover the same area as three robots would lead to many more localization 
errors on the single robot compared to those of any individual robot in the three-robot 


system. 
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Figure 24. Illustration of a two-robot exploration trial with the robots starting in widely 
different positions. 
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Figure 24 continued. Illustration of a two-robot exploration trial with the robots starting in 
widely different positions. 
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Figure 25. Illustration of a three-robot exploration trial with the robots starting in widely 
different positions. 
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Figure 25 continued. Illustration of a three-robot exploration trial with the robots starting 
in widely different positions. 


4. Counterproductive Effects 


Not all the results of using multiple robots were beneficial in terms of mapping 


accuracy and efficiency. There were several sets of circumstances that often led to 


multiple robot trials being less efficient than it would be assumed they would be and in 
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some cases even less efficient than a single robot system. The most common cause of 
multiple robot inefficiencies was near proximity of one robot to another robot during the 


exploration and mapping process. 


a. “Follow The Leader” Behavior 


Two related multiple robot exploration problems came to be known as the 
“Follow The Leader” and “Dancing Robots” behaviors. Both of these behaviors happen 
when two or more robots are near (within the trusted sonar range) one another. The 
‘Follow The Leader” behavior can be described as one robot appearing to follow another 
robot through the test environment and apparently mapping the same area that the leader 
robot has just mapped. Numerous real-life and simulation trials have revealed two 
predominant causative factors for this behavior. 

The primary cause seems to be the time delay in map data being passed 
from one robot to another and when that data is processed during the exploration routine. 
When two robots are near one another they will usually see the same frontiers nearby. 
Commonly one robot will proceed to a nearby frontier and the other robot will proceed to 
another nearby frontier that is slightly closer to it. However, if the second robot finishes 
its exploration of the first frontier and still has not received the map data from the first 
robot’s exploration there 1s a good chance that it will travel to the same frontier that the 
first robot just explored. 

The second robot will probably not receive and process the first robot’s 


map data until after the second robot has already mapped the same area that the first 
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robot just left. By then the first robot has moved on to a nearby frontier, which is now 
also the next frontier that the second robot will attempt to explore. This process can 
continue with the second robot always one set of map data behind the first robot and 
following it all over the test area. 

The other common cause of this behavior is that the second robot senses 
the first robot that is nearby as an obstacle in the environment with unexplored frontiers 
around it. While the second robot heads for the first robot’s position the second robot 
moves away to explore a nearby frontier. Once the second robot reaches the first (leader) 
robot’s previous position it makes a sensor sweep, again notes the nearby first robot as 
an obstacle with new frontiers to explore around it, and again the “Follow The Leader” 
process continues. In the best case this type of behavior merely reduces the effectiveness 
of the system by one robot (the following robot). In the worse case, instead of the 
“Follow The Leader” behavior, the “Dancing Robots” behavior occurs and both robots are 


rendered ineffective. 


b. “Dancing Robots” Behavior 


The “Dancing Robots” behavior can be described as two or more robots 
circling around or moving back and forth near one another for extended period of time and 
remaining in a relatively small area of the test environment. One variant on this behavior 
is vaguely reminiscent of the “do-si-do” movement, typically seen in square dancing, 


where two robots will swap places as if they are swinging each other around. As 


1Q2 


interesting as this behavior is to observe, it does not aid in the exploration and mapping 
process. 

This behavior is also caused by a robot sensing another robot as an 
obstacle with new frontiers around it to be explored. However, in this case both robots 
sense one another instead of just a follower sensing a leader. Whenever one of them 
moves to explore the frontiers around the other, the other does the same and thus the 
robots “dance” around one another. This process can continue indefinitely until one 
robot is stopped or another nearby frontier that is not caused by a mobile robot is chosen 
for exploration. Besides keeping two robots from exploring the rest of the area, the 
constant “dancing” motions have an extremely detrimental effect on localization. 

Figure 26 is an example of this inadvertently happening in a three-robot 
trial. The robots were initially started at positions one, six and seven. Exploration 
continued normally for the first few minutes. After about 5-7 minutes the robots in the 
two lower corridors came close enough together to sense one another. At that point many 
small frontiers were generated by each robot during its exploration process as the other 
robot moved through the area while the sonar sensor sweep was taking place. The robots 
began to “dance” around one another for the next 6-7 minutes until one of them was 
halted. At that point the other robot was able to “break free” because no new frontiers 
were being generated by a moving robot. However, by this point the robot’s localization 


has been compromised due to the effects of small movements around the other robot. 
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Figure 26. Illustration of “Dancing Robots” behavior during a three-robot trial. 
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Figure 26 continued. Illustration of “Dancing Robots” behavior during a three-robot 
trial. 


Figure 27 is a trial done with all the robots started in a very near proximity 
to one another. The initial starting points were positions one, two, and three. Nserver 
can be used to display the robots relative positions in the test environment based on the 
encoder data sent from the robot back to its respective controlling process. This option 
was used to track the robots’ positions in the test area. The robots are labeled on the 
figure for ease of reference. 

At the start the first robot is slightly farther away from the other robots 
and does not see any frontiers generated by detecting the other robots as obstacle. In 
comparison, the second and third robots are much closer together and generate many small 
frontiers as they detect each other nearby. At first it appeared that the second robot 
might just follow the third robot, but the third robot detected no frontiers closer than 


those around the second robot and thus began the “dance.” 
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Figure 27. “Dancing Robot” behavior from robots in near proximity. 
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Figure 27 continued. “Dancing Robot” behavior from robots in near proximity. 
cS Propagation of “Bad” Data 


Throughout all the multiple robot trials it was evident that a single robot 
could enter “bad” data into the system. Localization reliability varied greatly from robot 
to robot and trial run to trial run depending on many variable such as the area being 


mapped, wheel slippage, etc. As seen in many of the multiple robot results shown here a 
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map made by multiple robots can be very accurate in some areas and very inaccurate in 


others depending on the varying quality of mapping data sent from the individual robots. 


d, Network Reliability Problems 


Throughout the trial runs there were unexplained network problems that 
seemed to increase in severity as the number of robots used was increased. Despite many 
attempts to track and mitigate the problem they continued to greatly detract from the 
ability to operate three or more robots for extended periods of time. For three robots 20 
minutes was normally the longest time the robots would operate before packet errors 


caused termination of the experiment. This problem remains under investigation. 


C: LESSONS LEARNED 


There were several immediate lessons learned from this initial research. The first 
of these was that rotational odometry errors are much more detrimental to mapping 
efforts than translational errors. While translational errors do affect the quality of the 
map the robot is still able to navigate. Rotational odometry errors quickly increase to the 
point that that the robot is completely confused as to which direction it is facing and 
further navigation becomes impossible. However, any rotational localization scheme 
(such as wall or corner detection) will probably have a beneficial side effect of aiding 
translational localization as well. 

The second lesson is that the better a robot can explore and map on an individual 


basis the better it will function as part of a multiple robot exploration and mapping 
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system. This 1s basically common sense. Continued improvements in single robot 
mapping will also improve multiple robot mapping. 

The third lesson 1s that the network reliability issue needs further investigation. It 
needs to be determined whether the robot trials were causing the problem or if the cause 
was from an outside source. As mentioned above the local network administrators are 


currently investigating this problem. 
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VIL RECOMMENDATIONS FOR FURTHER STUDY 


Due to the extensive amount of software modifications necessary and the 
constrained equipment availability there were a number of areas of research which 
promised to be very interesting, but which there was not time to pursue. It is hoped that 
future students will take up the task of continuing some of the possible avenues 
mentioned here now that the initial work has been done in order to provide a testbed 
system for research. These future research possibilities can be broken up into two main 
categories: those that would involve mainly software modifications only and those that 


would involve hardware additions or modifications in addition to software changes. 


A. SOFTWARE CHANGES ONLY 


Many possible research areas would require only software changes to the existing 
code and require no additional hardware. Also, there is still ample opportunity for 


optimization of the existing frontier-based exploration routines as currently implemented. 


1. Centralized Map Building Process 


There are many possible methods to centralize the map building process and 
possibly reduce or eliminate some of the counterproductive behavior seen in the initial 
trials while still allowing the individual robot processes to function with relative 
autonomy. One possibility is to implement a sort of “Blackboard” to which the 


individual robot processes would write, or send, map information. 


eet 


The Blackboard would be a separate process or perhaps a “virtual” robot that 
would only accept local remote maps from all the robots and control no individual robot 
of its own. It would use the local maps sent to it to build a global map, which could then 
be sent back to the individual robot processes. It would take the place of the of the first 
robot process in the current implementation, acting as a server with all the individual 
robot processes as clients to it. How this might look for a system with four individual 


robots is illustrated in Figure 28. 


Blackboard Process To and From Individual Robot Processes 


Blackboard 
Process 


Robot Robot Robot Robot 
Process Process Process Process 
1 2 2 2 





Figure 28. Illustration of a Blackboard-type process interacting with four individual robot 
processes. 


This procedure could have a beneficial effect on the map building process ina 
number of different ways. By using only local scans to build the global map many of the 
problems of temporary obstacles gaining persistence and the unwanted reinforcement of 


“noisy” data are mitigated if not eliminated. Also, since the Blackboard process would 
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not be controlling a robot directly it can scan the shared memory location or “listen” for 
new local map messages constantly. This eliminates entirely the problem of missing local 
updates and losing map information from the individual robots. The Blackboard process 
would also provide a central point from which a user or operator could monitor the 
robotic mapping efforts of both the system and individual robots. 

The Blackboard process could also be used to track the location of individual 
robots. Using this information the global map could be marked to either show the area 
physically covered by a robot as obstacle free or already explored. When this global map 
is sent back to the individual robots this information could be used to eliminate the 
problem of robots sensing each other as obstacles and the “dancing robot” behavior that 
follows. Having the Blackboard process create the global map also makes for a 
convenient location for any new robots joining the system to find out the most current 
map and avoid duplication of earlier efforts. More of the challenges of managing a 
dynamic robot population are discussed below. 

The basics of the Blackboard process should actually require very little coding. 
Most of the functionality of taking 1n local maps and creating a global map is already done 
in the current implementation by the first robot process when acting as a server. Also, 
the client robot processes already write their local maps to and read the global map from 
the same shared memory location. For a basic Blackboard process simply removing the 
code that controls an individual robot from the original robot code and having the process 


constantly scan for and process local maps from the client processes would be sufficient. 


Hels 


2: Centrally Coordinated Effort 


Another aspect that is worth investigating is removing some autonomy from the 
individual robot processes and creating some sort of centralized control or supervisory 
function. In this case the supervisory process would be able to direct or at least influence 
the individual robots’ actions. This control could be constant, thus removing all 
autonomy from the individual robot processes, or on an as needed or exceptional basis, 
otherwise allowing the robots to act independently. 

This control process would most likely act in conjunction with some sort of robot 
tracking process, perhaps one much like the Blackboard process described above. Besides 
eliminating the “Dancing Robots” and “Follow The Leader” behaviors it would also 
provide a single point from which an outside user or operator could direct the actions of 
an individual or groups of robots as well as see the results of the robotic mapping efforts. 
This is an important option in a deployable reconnaissance system. 

This modification of the original implementation would require more extensive 
changes than just adding a simple tracking system. Besides the creating the supervisory 
process itself, it would also be necessary to make extensive modifications to the 


individual robot processes to have them accept commands from an outside source. 


3: Dynamic Robot Population 


The current implementation does not allow for an easy or simple method of 


joining additional robots to the system after initialization or for a way for a robot to 
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gracefully leave the system (say to go on to another task or report for maintenance). 
What is needed is some way to easily manage a dynamic or changing robot population. 

Whenever a robot enters the system it needs some sort of unique identifier within 
the system so that other robots and any supervisory or other processes that exist have a 
way to identify or track the new robot. This identifier also serves to identify any local 
maps made by the robot. Under the current implementation the robot identifier is 
assigned by the Nserver in the order that the robots are created in the program. The user 
then assigns the individual robot processes a number corresponding to the one given by 
the Nserver program. For a dynamic robot population a dynamic method of allocating 
unique identifiers to robots is required. 

One possibility would be a to use a simple Boolean array stored in the shared 
memory location used by all the robots. The size of the array would be the maximum 
number of robots that the system could manage. The array would be initialized to all 
zeros representing no robots in the system. As robots enter the system they would first 
scan the array until they found the first zero position. The robot would set the zero to a 
one and the numeric position in the array of that zero would become the robot’s unique 
identifier. 

Likewise, a robot leaving the system would reset its identifier position in the array 
to a zero, thus opening up that identifier for a new robot joining the system to use. If the 
system 1s filled with all the robots it can use or manage new robots would find the array 
filled with ones and would act appropriately depending on the system design, either 


waiting until an opening is available, moving on, or taking some other action altogether. 


int 


How this might function for a system with a maximum capacity of four robots is 


illustrated in Figure 29. 


System With Four Robot Maximum Capacity 
1 23 4 


Robot Identifier Array At Initialization o}ofo}o 


First Robot Enters System and Takes First Identifier 


Becoming Robot One [ifofofo] 


Process Continues Until Four Robots Are In System 
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l 


And Becomes Robot Three fi fafa]i| 


1 23 4 
Jfofolo 
: 
2 
-Any New Robots Attempting To Enter At This Time ee 
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2 3 4 
ififoti 
2 
fifita 





Figure 29. Illustration of the use of a Robot Identifier Array in the managing of a system 
with a four robot limit. 


Furthermore, this array could hold other information rather than just the individual 
robot identifier data. In conjunction with the sort of supervisory process described above 
it might also be useful to have additional information such as sensor types and ranges 
available on the robot, mobility platform capabilities and limitations, and other types of 
data that could aid the central controlling process in managing the robotic resources 


available to it. 
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4. Communications Networking Model 


The communications model in the current implementation is based on a point-to- 
point system in which an individual robot process explicitly communicates with only one 
other robot process at a time across a hardwired network connection. To better simulate a 
deployable system, where all links are wireless, a broadcast communications model 
should be used. One possibility would be to simulate the effects of range loss by 
including the individual robot’s coordinates in the global map as an attachment to any 
message. The receiving robot could compare the sending robot’s location to its own and 
decide whether or not to “accept” the message based on the distance between the two 
robots. 

Another area worth further research is the general appropriateness of TCP as a 
communications protocol for mobile robot systems. Mobile robots in a real world 
wireless environment do not fit well with the design behind of TCP and its onentation 
around continuous streams of data. Mobile robots require a more message-based 
communications design. There are a number of other possible networking models and 
protocols other than the TCP/IP model currently used. Some work in this area has 


already been done, focusing in on the use of the User Datagram Protocol. [Ref. 33] 


5. Improved Localization Method 


As has already been mentioned the current implementation has no method of 


localization beyond simple dead reckoning using the robot’s on-board odometric systems. 
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As shown in Chapter VI, this quickly proves insufficient as a means to accurately track 
the robot’s location and map making efforts suffer accordingly. There has already been 
much work done with localization routines ona NOMAD 200 robot at NRL, both alone 
and in conjunction with frontier-based exploration [Ref. 20, 22]. It is hoped that their 
methods might be adapted to work with the NOMAD SCOUT robot as well. 

Other research at NPS has concentrated on determining a robot’s location in the 
real world through interpretation of its surroundings [Ref. 8, 9]. This work also shows 
promise of forming the basis of a general localization routine for any number or type of 
mobile robots. Other efforts have attempted to have the robot match its current 
surroundings to an evidence grid built by the robot and correct any rotational or 
translational errors that may have developed [Ref. 34]. 

Another possibility involves the outside use of some sort of supervisory process 
such as described above. This process could monitor a robot’s self-reported location, the 
robot’s reported surrounding, as well as any sensor reports from other robot’s in the area. 
Using this information the supervisory process might track each of the robots in the 
system and send correction information to them as their dead reckoning systems begin to 
drift. Still another possibility is a group of robots forming a local reference system 
without the use of any central process. Some work has already been done in this area 


[Ref. 35]. 
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6. Managing a Heterogeneous Robot Population 


The multiple robot frontier-based exploration system has been implemented 
separately on groups of NOMAD 200 and NOMAD SCOUT robots. So far there has 
been no work done on integrating a heterogeneous population of robots in such a system. 
There are important questions concerning the code base for such a system. Would it be 
better (or even feasible) to write a single set of code that would adequately work with the 
varying sensor systems and mobility features of each platform? Or would it be better to 
have two completely different sets of routines for each type of robot? - 

An interesting aspect of a heterogeneous population of robots is the varying 
capabilities of each type of robot. While the NOMAD 200 has a more precise 
positioning capability, the NOMAD SCOUT is somewhat smaller and may be able to 
explore spaces the NOMAD 200 cannot reach. It would be interesting to find a way to 
use the diversity of the robot types as an advantage in accurate and complete exploration 
and map making. Both NRL and NPS now have both types of robots, thus providing a 


basis for such work. Some theoretical work has already been done in this area [Ref. 36]. 


Pe Identifying System Tradeoffs 


Initial research has already identified some of the tradeoffs of optimizing or 
adjusting various aspects of the system. For instance, reducing the “trustworthy” sonar 
range results in much less problems with specular reflections, but increases the total 


distance a robot or number of robots must travel in order to map a given area. This 
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increased travel distance, especially the larger number of small movements necessary to 
sweep the sonar sensors at new frontiers, leads to increased odometry error. On the other 
hand, increasing the “trusted” sonar range reduces travel requirements, but causes a 
corresponding increase in false sonar returns. 

Much more study is needed on optimizing the sensor model for the best 
combination of accurate mapping with minimal movement. Of course an adequate 
localization routine would solve many problems, but even with a good localization routine 
minimizing travel distances would be beneficial to the overall system. Other possible 
tradeoff studies include autonomy versus centralized control and heterogeneous versus 
homogeneous robot populations. Some work has already been done in the study of the 
interaction between quantity of robots in a system, sensor quality, and mobility 


constraints on system performance for a given mission [Ref. 37]. 


8. Modified Movement Behaviors 


The original movement behaviors for the robot in the routine robot.cc were written 
with the capabilities of the NOMAD 200 robot in mind. Problems arise with using these 
same behaviors on the NOMAD SCOUT robot. While the NOMAD 200 can translate 
on its own axis, the NOMAD SCOUT cannot. Therefore movement behaviors that 
would have been simple rotations or shallow arcs on a NOMAD 200 become much larger 
movements when the conversion macros interpret them for use on the NOMAD SCOUT. 

This causes many difficulties when the NOMAD SCOUT is near to and facing a 


wall or other obstacle at the end of a sensor sweep. When the robot tries to move on to a 
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new frontier the macro translates the current movement commands into a forward turning 
motion. Ifthe robot is too close to the wall it will be blocked and eventually the new 
frontier it should have traveled to will be marked as inaccessible. Some type of backing 
up or modified turn behavior would seem to be a possible solution. 

Other opportunities also exist to counter the “Follow The Leader” and “Dancing 
Robot” behaviors. One possible solution to be explored might be to have the robot 
broadcast its location either to all robots in the system or at least those nearby. That way 
a robot could recognize that the obstacle it is trying to map is in fact another robot. Of 
course any sort of centralized supervisory process could also counter this problem very 
easily. Another possibility 1s to set some sort of limit on how long a robot would 
continue to map a small area despite new frontiers constantly appearing in that area. 
After a period of time it could give up and move on to a radically different geographical 


area. 


B. HARDWARE AND SOFTWARE CHANGES 


Other possible research areas would require additional hardware and/or 
modification of the already existing hardware in the system. In addition, software 
modifications would be necessary in order to use the added or changed hardware. Some of 


the hardware for the research possibilities mentioned below is already available at NPS. 
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1. Human — Robotic System Interaction 


The area of real-time human-robotic interaction holds many, many possible 
research opportunities. In the current implementation the current map is displayed on a 
workstation and the opportunities for user interaction are very limited. Obviously, for a 
practical, deployable system these limitations must be removed. 

There is a need for some sort of portable system through which a user can receive 
information from a single robot or groups of robots and also direct the actions of an 
individual robot or number of robots. Ideally, the device would be lightweight, 
unobtrusive, and user friendly in design and use. While earlier research had focused on 
rather large control and display systems [Ref. 38], perhaps the best model available today 
for such a device is the in the form factor and design of a Personal Digital Assistant 
(PDA). 

There are a number of different types of PDAs available for research at NPS. 
Many of them have some type of wireless connection or network capability. What is 
required is a method for a map data to be transmitted to these devices in a useable format 
and some method for operator commands to be sent back to the network and then on to 
the robot(s). Once this is possible many other research opportunities become available. 
What is the best way to manage the system from the operator point-of-view? Fora 
deployed system, how much control does the operator need or even want? Is controlling 
the robot(s) the operator’s primary duty or something that should be done on an as- 


required basis? What are the possibilities for cooperative exploration of an area between 
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human and robot? How best should the system signal the operator to possible danger 
areas or other areas of interest? These and other questions should be investigated early 
before large amounts of funding are spent on programs that later are found to be 


unworkable or impractical to implement. 


Ze Outdoor Trials 


Any practical, field deployable system will need to work outdoors as well 
indoors. Even if the system is build primarily for mapping the interiors of buildings the 
individual robots will most likely have to traverse rough, broken, urban terrain to travel 
from one operating site to another. Neither the NOMAD 200 nor the NOMAD SCOUT 
has much capability in this regard as currently configured. At this time their manufacturer 
has no announced plans to market any outdoor-capable models either. 

However, there does exist other outdoor-capable robotic systems at NPS. In 
particular the “Shepherd” vehicle [Ref. 39, 40], under cooperative development by 
several different departments at NPS, is a robot with a four-wheeled all-terrain-vehicle 
(ATV) style chassis with independent driving and steering capability. Much study has 
been conducted on this platform concerning motion control and localization via inertial 
sensors and the use of a Global Positioning System (GPS) receiver. It would seem that 
integrating some sort of mapping capability into it would be a natural next step. There 
are many questions about how much of the current implementation of the frontier-based 
exploration code could be ported to the new platform, but the fundamentals of the 


process would seem to remain the same. 


3. Removing Dependency on Wired Network 


Perhaps one of the most ambitious possibilities is removing the dependency that 
the current implementation has on a wired network to provide communications 
connectivity. The NOMAD SCOUT has the capability to be completely independent 
through the use of a laptop computer running the LINUX operating system. A laptop 
can be mounted on top of the NOMAD SCOUT and can run the same code locally (after 
it is recompiled) that is currently run on a remote Sun workstation. 

Once the NOMAD SCOUT robots are operating independently of a wired 
network it might be possible to better implement a broadcast model of communication 
amongst the robots. The wireless modems used in the current implementation are capable 
of serving as either point-to-point communications stations or as part of a distributed 
system. Because there would no longer be a shared memory location, the exploration and 
mapping software would have to be modified to actually send all the map data and not 
just a pointer to the data or message that it is available to the other robots in the system. 
There is already a body of work supporting communications protocols for distributed 


robotic systems without a centralized communications server [Ref. 41]. 


4. Additional Sensor Systems 


Using laptops on the NOMAD SCOUT robots as mentioned above also opens up 
the opportunity to integrate additional sensor systems on the platform. The unused 


input ports of the laptop provide a means to include video, audio, or any of a number of 


124 


other sensing devices to the system. In addition, there is also the possibility of adding a 
means of detecting beacons in the environment or on other robots as an aid to navigation 


and localization. 
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VI. CONCLUSIONS 


Oftentimes the hardest part of any journey is just getting started. This thesis and 
the research involved in creating it have been aimed at creating a starting point for future 
studies. Now that the basics of a real world (as compared to simulated) multiple robot 
system has been developed and implemented at NPS, a huge number of additional avenues 
of investigation are available. 

It has been shown that much work remains in order to create a consistent and 
robust exploration and mapping system before many of the questions surrounding robotic 
battlefield support can be answered. However, now there exists at NPS a “critical mass” 
of mobile robot types with varying capabilities and at least basic software to enable some 
of them to operate in a shared real-world environment toward a common goal. 

Many of the problems encountered in this research are very similar to those 
discovered by other researchers when moving a robotic system from one environment to 
another [Ref. 42]. In the case of this research the testing of multiple mobile robots has 
been moved from simulation-only environment to a combination of simulation and real- 
world testing. This transition has revealed many details of multiple robotic systems that 
otherwise would have remained hidden in simulator-only testing. 

In Chapter VI there are listed many possible areas of study based on the work 
presented here. These are just the start of many possible thesis opportunities involving 
hardware, software, human-machine interaction, etc. One of the most exciting and 


challenging things about robotics as a field of study is the number of different fields and 
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disciplines that 1t encompasses. It 1s hoped that future students will take up this 


challenge and carry on the work started here. 


128 


O COIN ABW DO 


APPENDIX A. SOURCE CODE FOR COLLECTION OF SIMPLE SENSOR 
RETURN DATA 


This appendix contains the source code that was used to collect sonar range data on early 
map making efforts with the NOMAD SCOUT robot. 


Ve Renee Re eee ee Ee eR eee ee ee ee ee ee Ae 


* 


PROGRAM: world sonar.c 


* 
* 
* PURPOSE: To collect sonar data for establishing a world map. 
* modified for Scout by Patrick A. Hillmeyer 

* 


a ee RA ee eee ee ee eee ee eS 


/*** Include Files ***/ 


fiancilude “Nelient wh" 
ftinclude <stdio.h> 
Himelide <stdlib.h> 
#include <math.h> 


/*** Conversion MACROS courtesy of Nomadic Inc ***/ 
i Original beta macros for SCOUT models: *~***/ 


#define RIGHT(trans, steer) (trans + (int) ((float) steer* 368.61/3600.0)) 


#define LEFT(trans, steer) (Crans,= (1nt),( (float )steer*soo7o) 5500.0) 7 
#define scout _vm(trans, steer) vm(RIGHT(trans, steer), LEFT(trans, 
steer), Q) 

#define scout pr(trans, steer) pr(RIGHT(trans, steer), LEFT(trans, 


Steer), QO) ° 


PS Function: Protocypes ***/ 


void GetSensorData (void); 


p= TeLoboas 27-7 


long SonarRange{[ 16}; /* array of sonar readings (inches) */ 
long IRRange[ 16}; /* array of infrared readings (no units) */ 
long robot —conkig| 4); 


i Moat nee Gog i atk t “aa 


main (unsigned int argc, char** argv) 
{ 

AVE ye yap yee Cle x 

int order[ 16] ; 

FILE * fp; 


Ie 


/* Connect to Nserver. The parameter passed must always be 1. */ 
connect robot(l, MOBEL SCOUlL, ’ Scouti@e-e-nes. may. mi je a eee, 


/* Initialize Smask and send to robot. Smask is a large array that 
controls which data the robot returns back to the server. This 
function tells the robot to give us everything. */ 

inte mask () > 


/* Configure timeout (given in seconds). This is how long the robot 
will keep moving if you become disconnected. Set this low if there 
are walls nearby. */ 

eonk (ined). 


/* Sonar setup */ 

for (i = 0; i < 16; i++) 
order[ i] = i; 

Cont shtlo, order); 


mrt{)e /* tell robot to. zero itselt */ 


fp = fopen("range.dat", "w"); 


/* Main loop. */ 
FOr Mia—O Poe 2) 
{ 


GetSensorData(); 


for (j=0; 3<16; j++) 

fFOorintt (ip, “Ssdessd s¢descad sod. \n", 
rebot -contig| 0) Fobot, contr 1] 1obot contre); 
EODOE -COMEI CG] 31, 
SonarRange[ 3] ); 


} 
EClOose(to); 


/* Daseconmeck.. -7 
da Sconnect Topo: (lr, 


GetSensorData(). Read in sensor data and load into arrays. */ 


void GetSensorData (void) 


Tae 


/* Read all sensors and load data into State array. */ 
gst); 
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/* Read State array data and put readings into individual arrays. */ 
FOr (ee <a) 
{ 
/* Sonar ranges are given in inches, and can be between 6 and 
255) aaclusive. 77 
SonarRange[ i] = State[ 17+i] ; 


/* IR readings are between 0 and 15, inclusive. This value is 
inversely proportional to the light reflected by the detected 
object, and is thus proportional to the distance of the 
object. Due to the many environmental variables effecting the 
reflectance of infrared light, distances cannot be accurately 
ascribed to the IR readings. */ 

IRRangef i] = State 1+i] ; 


HOE (i = O07 1 < 4; 34+) 
robot config! 1] = State! 34434] ; 


hey 





OO OO~JTA NAR WHN 
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APPENDIX B. MATLAB SOURCE CODE FOR PLOTTING OF SIMPLE 
SENSOR RETURN DATA 


This appendix contains the MATLAB M-file that was used to analyze and display the 
sonar range data from early map making efforts with the NOMAD SCOUT robot. 


oP 


Capt Patrick A. Hillmeyer, USMC 

Code originally written for EC 4300 Robotics class 
Written to interpret sonar range data collected 
from a NOMAD SCOUT robot 


oo 


oP oP? 


Load the range data collected during the robot's travel 
oad rangel.dat 


kK oe 


xobo data=rengel; 


%$ Convert robot x,y coordinates to inches 
rob _x_in world=robo_data(:,1)/10; 
rob _y_ in world=robo data(:,2)/10; 


In this data the base and turret are aligned 
therefore no alignment correction is necessary 
Carryover from old NOMAD 200 version of code 


oP oP oO 


$ covert angles to degrees then to radians 
base angle=(robo data(:,3)/10)*pi/180; 
Obgmalst fr reb=robo data(:, 5); 


num sensors=16; 
deg per sensor=360/num_ sensors; 
rad_ per sensor=deg per sensor*pi/180; 


% correct for sensor location offset 
% from robot center 
noo madius—3.6 1; 


Set the range at which to trust the 
sonar data 
_trust=60; 


of oP 


W 


% plot robot path alone 

figure(1) 

Prottrob xX in world; rob yvin world, w.') 
title('Robot path in real (or simulated) world') 
xlabel('Inches'),ylabel('Inches') 

axis ('equal') 


now plot sonar hits as robot moved 
sonar hits=]; 


x 
yYoSonar hits=|]; 


oo 


Read through the data in sets corresponding 

to the number of sensor readings taken at each 
6 location in the robot's path 

for ctr2=0: ((length(robo_ data) /num_sensors) -1) 


oo 


for ctr3=l1:num sensors 
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abs data, pt=(num_seéensors* ctr2)+ctr3; 


% only process if valid reading 
if ‘Obg dist ir rob (absrddea pe)<s enuse 


A B T={ cos(base_angle(abs data pt) ) 
=sin(base angle(abs data pt) } 
O rob_x_ in world(abs data pt); 
sin(base angle(abs data pt))} 
cos(base_ angle(abs data pt)) 
O rob_y_ in world(abs data pt); 
O02 CG; 
O00 1); 


$ correct for off-by-one discrepency 
$ in sensor numbering 
sensor num=ctr3-1; 


B P= (obj dist _fr_ rob(abs_ data pt)+rob radius) 
COS (Sensor Nun Fad, per Sensor), 
(OD) (OVSE tr oroOb (abs data pt) trod ~adius) 
*sin(sensor _num* rad per sensor); 
0; 

1); 


A P=A_B T*B P; 


% Sonar hits={ x sonar hits A P(1)]; 
Yo sonar Naess yy sonde hics & F(Z) 


end % end for if 
end % end for ctr3 
end % end for ctr2 
figure (2) 
plot(s sonar hits, y Sonar nats, wey eae 
reo x an world, robsy in world, ww.) 
title('Simulated world sonar data - 60 inch sonar reliability') 


xlabel ('Inches'),ylabel('Inches') 
axis('equal') 
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APPENDIX C. FRONTIER-BASED EXPLORATION CODE —- GRID.H 


This appendix contains the header file for the routine that builds the evidence grid based 


on the sensor return data. 
/* 
gGridsn 


Header file for robot/evidence grid functions 
original code by Brian Yamauchi 


Modatiecations for SsCOUl THESIS 
by Patrick A. Hillmeyer 


*/, 


Pinclude “cmacs.h"’ 
#include "volsense.h" 


/* Grid occupied threshold */ 
#define GRID POS THRESH 16 

/* Grid unoccupied threshold */ 
#define GRID NEG THRESH -16 

/* Local Grid dimensions (feet) */ 


/* BEGIN SCOUT THESIS CHANGE */ 


/* change the local grid dimensions to match the global grid dimensions 


2), 

#define X MIN -22.0 
#define X_MAX 22.0 
#define Y MIN -22.0 
#define Y MAX 22.0 
#define Z MIN 0.0 
#define Z MAX 5.0 


/* Grid resolution (cells) */ 
/* increase the number of cells */ 


/* the value here has to be a power of 2 and symetrical 


jr wei er e4aby 64, 1287 5y.128 ..etc ed 


ee 


/* this is true for all the other grid resolutions below as well */ 


#define X RES 256 
#define Y RES 256 
#define Z RES 1 


/ SEND escOUT THESIS CHANCE w= 7 


/* Global grid dimensions (feet) */ 


#define GLOBAL X MIN -22.0 
#define GLOBAL X MAX 22.0 
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#define GLOBAL Y MIN -22.0 
#define GLOBAL Y MAX 22.0 
#define GLOBAL Z MIN 0.0 
#define GLOBAL Z MAX 5.0 


/* Global grid resolution (cells) */ 


#define GLOBAL X RES 256 
#define GLOBAL Y RES 256 
#define GLOBAL Z RES 1 


/* Navigation grid dimensions (feet) */ 


#define NAV_X MIN -22.0 
#define NAV _X MAX 22.0 
#define NAV _Y MIN -22.0 
#define NAV Y MAX 22.0 
#define NAV_Z MIN 0.0 
#define NAV_Z MAX 5.0 


J» Resolution of Navigation grid (cells) 477 


#define NAV_X_RES 256 
#define NAV_Y RES 256 
#define NAV _Z RES 1 


/* Sensor modes */ 


#define SONAR MODE 0 
#define LASER MODE 1 
#define INTEG MODE 2 


/* Sensor parameters */ 
/* BEGIN SCOUT THESIS CHANGE */ 


/* Scout and Scout2 dimensions 15.125 in sensor to sensor diameter */ 
/* Scout2 sonar height 10.25 in Scout close enough to use same value */ 
/* Height tzom floor oO: Sonar (ft)  Scoucs 10.25 1n--7 

#define SONAR _HEIGHT 0.8542 

J/* Offset from xrobot center to sonar (ft) Scouts 7.5625 7172-7 

#define SONAR RAD 0.63 


/* Separation between adjacent sonars (deg) - same as Nomad 200 */ 
#define SONAR SEP 22.5 

j* HMelgnt from floor to IR (ft) = Nene om Secoue 47 

#define TREBEEGHT 0.0 

f= *Ofiset, from robot center to IR (fl) == None cn Scout 7 

#define IR_RAD 0.0 

/* Separation between adjacent IR (deg) - None on Scout */ 


#define IR_SEP 0.0 
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/* Height from floor to laser (ft) - None on Scout */ 
#define LASER HEIGHT 0.0 

/* END SCOUT THESIS CHANGE */ 

#define HEIGHT OFFSET 0.0 /* Z-axis offsee (ft): */ 


/* Maximum sonar reading (indicates no reflection) */ 
#define MAX SONAR READING 255 


/* Maximum (valid) sonar range (feet) -- Use 21.25 for no truncation */ 
/* BEGIN SCOUT THESIS CHANGE */ 

/* This is the trustworthy range of the sonar in ft */ 

/* shorter range for Scout to reduce specular reflection problem * / 


#define MAX SONAR RANGE 8.0 


/*#define MAX SONAR RANGE 10.0* / 
/*#define MAX SONAR RANGE 21.25*/ 


/* Maximum sonar range for occupied cells (feet) */ 
/* This value seems to have no effect */ 

/*#define MAX SONAR OCC_RANGE 3.0*/ 

#define MAX SONAR _OCC_RANGE 15.0 

/* Maximum IR reading (indicates no reflection) */ 
#define MAX IR READING 0 {= Ne IR on Scout * 7 


/* Maximum (valid) laser range (feet) */ 


/*#define MAX LASER RANGE 100. 0* / 
#define MAX LASER RANGE 0.0 /* No laser on Scout */ 


Ve TEND: SCOUT THESIS CHANGE *7 

je *Size of Cell in Yobct window *7 
#define DISPLAY SCALE 56.25 

/* Angle conversion constants */ 


#define M RAD2DEG 57.29578 
#define M DEG2RAD 0.017453293 


/* Laser configuration parameters */ 


#define LASER MODE OFF 0x32 PEO Wl eal) Oral, les ee J? yo ae Seen a7, 
#define LASER MODE ON 0x33 come lego Opp line ce, (em XG past oS ety 
#define LINE 0x03 75 FORO 00M atk: / /* X,Y pairs for endpoints 
a 

#define THRESHHOLD 70 /* £4=30, £2.8=5, factory=20 */ 

#define WIDTH 40 j*uta—20,062.6=207) tactory=20 *~/ 

#define NUMDATA 120 /* Number of points returned */ 

#define AVG 1 /* Number of pixels averaged */ 


[OU Steosrze for orineing arid ~/ 
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#define PRINT STEP 1 


/* Robot size */ 
/* BEGIN SCOUT THESIS CHANGE */ 
/* Scout2 is taller use its value - 14 in */ 


/* Add bumper space for total radius */ 


/* Robot radius (feet) Scout sensor radius plus .756 in for bumpers* / 
#define ROBOT RADIUS 0.693 


/* Robot height (feet) use Scout2 14 in */ 
#define ROBOT HEIGHT 1.1667 


/* Size necessary for safe robot passage (feet) */ 
#define ROBOT PASSAGE RADIUS 0.7 /* Add small safety margin */ 


/* END SCOUT THESIS CHANGE */ 


/* Grid decay factor ~7 
#define GRID DECAY 8 
/* Grid translation parameters */ 
#define NUM TRANS 1 /* Number of translations in each 
Girection 
along each axis */ 
#define TRANS STEP 0.2 /* Size of each translation step (feet) */ 


/* Grid rotation parameters */ 


#define NUM_ROT 1 /* Number of rotations (in each direction) */ 
#define ROT STEP 2.0 /* Rotation step (degrees) */ 


/* Mimimum change in position (1/10 inch) to update */ 
#define MIN DELTA 46.88 


/* Relative weight of clear cells in fine grid to coarse grid conversion 
ry 


#define F2C_ CLEAR WT 1 


/* Relative weight of occupied cells in fine grid to coarse grid 
conversion */ 


#define F2C_OCC WT 4 


/* Maximum laser/sonar angle difference for laser-limited sonar 
(degrees) */ 


#define LLS MAX ANGLE DIFF 3.0 
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APPENDIX D. FRONTIER-BASED EXPLORATION CODE — GRID.C 


This appendix contains the source code for the routine that builds the evidence grid based 
on the sensor return data. 
/* 

Grid.C 


Robot/evidence grid functions 
Original code by Brian Yamauchi 


Mecigication- for SCOUT ZHESIS 
by Patrick A. Hillmeyer 
hh 


Famnecilude <stdio.h> 
#include <math.h> 
#include "Nclient.h" 
famelude "grid.h" 


double min3(double x, double y, double 2z) 


/* 
Return the minimum of three values 

ay 
{ 

double m; /* Minimum * / 

m= xX; 

er < m) 

m= y; 


m= Z; 
} 
return (mm): 


} 


int world2grid(Map3D map, double wx, double wy, double wz, 
Mites Ox, mint.) Oy ants G2. 
/* 
Return grid coordinates for location in world coordinates 
nf 
{ 


double xsize, ysize, zsize; je Size. Of Grad ceil +7 


if ((wx < map.lomy 0] ) 
(wy < map.lomvi 1)) |] 
(wz < map.lomyj 2] ) || 
ig Prine wOrlazgqricd: oo 
Cheerios in; 


|] (wx > map.himv[ Oj) |] 
(wy > map.himvyj 1] ) |] 

(WZ -emap.homy Z) ))o4 
ae 


Nt: (Si," Sipiot )) OUbGeOr range: —%ti: ti, ses be, 


WX, WY, WZ, map.lomv[ 0], map.himvj 0], map.lomv{ 1], 
map.himv[ 1], 
map.lomv[ 2], map.himv[{ 2] );*/ 
return(-1); 


} 
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109 
110 


xsize = (map.himvj 0] - map.lomv{ 0) ) / map.msize{ 0] ; 
ysize = (map.himvj 1) - map.lomvj[ 1)) / map.msizef 1) ; 
zsize = (map.himvj 2] - map.lomvj 2] ) / map.msizef 2] ; 


*gx = (int) ((wx - map.lomv[ 0] ) / xsize); 
*gy = (int) ((wy - map.lomv{ 1] ) / ysize); 
gz) = (ine). (4wz = map. tomy 2) + HEIGH! OErrse!) 7 zsize)-; 
if ((*gx < 0) II (*gx >= map.msize{ 0) ) || 
(“oy = ©) |, Gy == 'man.msizel i) hi 
(“oz <0) | (92° >= map.msi2e| 2))) 4 
/* printt ("world2grid: world location (%£, $£, %¢£) --> cell) tae 
Sd). GUE. Of Pange..n 5 Wx, Wy, W2, “Gx, “OV. G2 7.7 


return(-1); 


} 


return ( 1) 


} 


int world2index(Map3D map, double wx, double wy, double wz) 
/* 

Return grid cell index for location in world coordinates 
ai 
{ 


double xsize, ysize, zsize; /* Size of arid celt 47 
Mt O%)0dy, C2 /* Coordimates of grid cell. 7 
int index; /* Grid cell array index */ 


if ((wx < map.lomvj 0) ) || (wx > map.himvyj 0] ) || 

(wy < map.lomvi 1) ) || (wy > map.himvi 1)) || 

(wz <= map.lomvy 2Z],) ||| (wz > mapynimy 2) 4 
i DEINEL(” world2index (tf, £,0 Sf) Out Of range <tl: tc eer: 
26s e> WX, WY, WZ, map.lomv{ 0], map.himv{ 0), map.lomvj 1], 
Map. haimy 1); 

map.lomv{ 2), map.himv{ 2] );*/ 
return(-1l); 


} 


xS1ze 
ysize 
zsize 


(map.himv{ 0) - map.lomv[ 0) ) / map.msize[ 0] ; 
(map.himv[ 1] - map.lomv{ 1) ) / map.msize{ 1) ; 
(map.himv{ 2] - map.lomv{ 2] ) / map.msizef 2] ; 


gx = (int) ((wx - map.lomvj 0] ) 
gy (int) ((wy - map.lomv{ 1) ) 
gz = (int) ((wz - map.lomvi 2] 


/ xSize); 
7 Vsuze); 
+ HEGCHE OFESEE) / 2z81ze); 


index = gz * map.msizel 0] * map.msize[ 1] + gy * map.msizel 0] +367. 


1f ((index < 0) || (index >= map.msize{ 0) * map.msize{ 1) * 
map.msize[ 2] )) { 
fe printt ("world2index: world location (Sf, @f, %f£) —=> index) 4a) 


out of range.\n", wx, wy, WZ, index) ;*/ 
recurm(—1); 


} 


/* DPEInts£t (“workd2qrid> world location. (sf, s£, Sf) =-—> cel lia wee 
$d] <%d>.\n", wx, wy, WZ, 9x, gy, gz, index) ;*/ 
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return (index); 


} 


Would GridZworladiMaes) Map, Int Gx,;aauG Oy, ant oz, 
double *wx, double *wy, double *wz) 
/* 
Return world coordinates for location in grid coordinates 
ey 
{ 


double xsize, ysize, zsize; /* Size (Of ‘Grid cer *7 
/* tate CU eee 7, 
xsize = (map.himv{ 0) - map.lomv[ 0] ) / map.msize{ 0] ; 
ysize = (map-himv{ 1] - map.lomv{ 1])) / map.msize{ 1]; 
zsize = (map.himv{ 2] - map.lomv{ 2]) / map.msize{ 2] ; 
“wx = (double) (gx + 0.5) * xsize + map.lomvy 0] ; 
*wy = (double) (gy + 0.5) * ysize + map.lomv{ lj; 
“wz = (Ocuble) (gz + CU. * zsize + map.lomv{ 2] ; 
f= de (worldZgridimap, “wx, *wy,6*Wz, (6tx, ty, 662) == —1Lg 
Brintt( <td iG, ers ——> (2f,0 ta, $f) ==> <7 22, 222 Pee en 
Gin dy, G25. “wx, * WY, SWZ); 
} 
else { 
Pernt" < ed) sa, tae ——> (sre Sf, Sf) ==> <td, sd, sade n'y 
Gy "“OV7- G2,0" WX, Wy, 7 WZ, tx, ty, Ez); 
ery 


} 


iieemicdZindex(Map3bemap, int gk, Int gy, int gz) 
a 

Return grid cell index for grid cell coordinates 
nay 
{ 


imMcea nde x; /* Grid cell array index 7 


index = gz * map.msize[{ 0] * map.msize{ 1] + gy * map.msize{ 0] + gx; 
return (index); 


} 


void set _location(Map3D map, double x, double y, double z, int value) 
/* 
set probability of grid cell corresponding to world location 
ay 
{ 
int gindex; /* Grid array index */ 


gindex = world2index(map, x, y, Z); 
1 ACG neexes 1 4 
map.mapm[ gindex] = value; 
} 
} 


Void Set grid(Map3D map; int x, int y,int 2, intsvalue) 


/* 


14] 


169 Set probability of specified grid cell 


170), 223/ 
171 { 
ce int gindex; /* Grid areay ime, = / 
17 
174 gindex = z * map.msize[{ 0] * map.msize{[ 1] + y * map.msize[ 0) + x; 
17 if ({gindex <0) |eigqindex >= map.msizel 0). * map-mseizei je 
176 map.msize[ 2])) { 
iy 8 86f* printf("set grid: cell [%d, %d, %d]J out of range <%d, %d, %d>.\n", 
178 X, VY, Z, Map.msizef 0], map.msize{ 1], map.msizel[ 2] ) ;* 
179 return; 
180 } 
181 map.mapm{ gindex] = value; 
182} 
183 
184 void grid init (Map3D *mapl, /* Grid pointer */ 
185 double cx, /* Center x-coord (feet) */ 
fee double cy) /* Center y-coord (feet) */ 
a 
188 Initialize evidence grid 
189+ / 
190 ¢ 
19] double lov{ 3], hiv{ 3]; Js 2Gricvcormners. (secs. 
a ine msizer oll: JF "Grid size (cella, 
194 mapl->cx = cx; 
195 mapl->cy = cy; 
196 
197 msize[ 0] = X_RES; 
198 lov{ 0] = cx + X MIN; 
199 hiv[ 0] = cx + X_MAX; 
200 
201 msize{ 1] = Y_ RES; 
202 Lovybl] “= cy + Yoein, 
203 hivl 1) = cy + Y_MAX; 
204 
205 msize{ 2] = Z RES; 
206 lov{ 2] = Z MIN; 
207 hiv[ 2] = Z MAX; 
208 
209 MakeMap3D(msize, lov, hiv, mapl); 
21} 
211 
212 void grid_init global(Map3D *mapl, /* Grid pointer */ 
213 double cx, /* Center x-coord (feet) */ 
214 double cy) /* Center y~-coord (feet) */ 
2N'5 i 
216 Initialize global evidence grid 
8 7 
ONS { 
219 double lov{ 3], hiv{ 3]; /* Grid corners (feet) */ 
oe int msize{ 3]; /* Grid size (cells) */ 
DPD mapl->cx = Cx; 
Zoo mapl->cy = cy; 
224 
225 msize[ 0) = GLOBAL X RES; 
226 lov{ 0] = cx + GLOBAL X MIN; 
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oy 
228 
229 
230 
231 
232 
233 
234 
D3. 
236 
237 
238 
239 


241 
242 
243 
244 
245 
246 
247 
248 
249 
250 
2S 1 
252 
D3 
254 
Zo) 
256 
ZS] 
258 
Zoo 
260 
261 
262 
263 
264 
265 


267 
268 


Z/0 
Z| 
ap 
273 
274 
ZiS 
276 
Ag 
278 
Z79 
280 
281 
282 
283 
284 


hiv{ 0] = cx + GLOBAL X MAX; 


msize{ 1] = GLOBAL Y_ RES; 
Lov[ 1} = cy + GLOBAL Y MIN; 
hivi 1] = cy + GLOBAL Y MAX; 


msize{[ 2} = GLOBAL 2 RES; 
lov{ 2} = GLOBAL _Z MIN; 
hiv{ 2] GLOBAL Z MAX; 


MakeMap3D(msize, lov, hiv, mapl); 
} 


vord grid init Nav (Map3sD “map, /* “Grid pointer */ 
double cx, /* Center x-coord (feet) */ 
double cy) /* Center y-coord (feet) */ 
jr 
Initialize evidence grid for navigation 
af 
{ 
double lov{ 3], hiv{ 3]; /* Grid corners (feet) */ 
int msizef{ 3}; /* Grid size (cells) */ 
mapl->cx = Cx; 
mapl->cy = cy; 


msize[ 0} = NAV X_ RES; 
lov{[ 0) = cx + NAV X MIN; 
hivl 0) = cx + NAV_X MAX; 


msize{ 1} = NAV_Y_RES; 


lov{f 1] = cy + NAV_Y MIN; 
havi) = cy + NAV_Y MAX; 
msize{ 2] = NAV _Z RES; 
lov{[ 2] = NAV_Z MIN; 

hiv{ 2] = NAV_Z MAX; 


MakeMap3D(msize, lov, hiv, mapl); 
} 


VOud Gridvprint (Mao3D map, siInt yaxis) 


/* 
Print evidence grid occupancy probabilities 
a 
{ 
UNC e Ke. Jeez: /* Cell index */ 
int xsize, ysize, zsize; /* Grid dimensions (# cells) */ 
ree /* OCeUpancy pEObabiiaty */ 
int empty; /* Empty level flag */ 
xsize map.msizef[ 0] ; 


ysize = map.msizef 1]; 


zsize map.msizef 2] ; 

for (z = 0; z < zsize; ztt) { 
y=x =0; 
empty = 1; 
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while((y < ysize) && (x < xsize) && empty) 
if (map.mapn{ z * xsize * ysize + y * xsize 


empty = 0; 
} 
xXt+; 
if (x == xsize) { 
x “= 0; 
ytt; 
} 
} 


if (!empty) { 
printt( "Level: Sd\nvn", 2); 


for (y =80; yo <-ysize- vit) { 
for (x = 0; x < xsize; x+t) { 
if (yaxis == 1) { 


p= map. mapni Z* xsi ze_* ysize +r (ysize -y- 1) * xsize + x]; 


} 


else { 


{ 


p = map.mapn[ z * xsize * ysize + y * xsize + x]; 


} 
Bote) Ot 
Srintt (eo: 
} 
else if (p == 0) { 
Gee Ao er iiy 
} 
else if (p > -25) { 
Drie Ce 
} 
else if (p> -50) { 
Drier Go 
} 
else { 
Oe rie er 
} 
} 
PramerG: wae). 
} 
getcnar 
} 
} 
} 


void sonar print (Map3D map, int yaxis) 


j* 


Print evidence grid occupancy probabilities for sonar level 


a 
{ 
int x, ye Zz. /* Geli amdex 27 
int xsize, ysize, zsize; /* Grid dimensions (# cells) 
int p; /* OcClpency probabil licy =/7 
int empty; /* Empty level flag */ 
xsize map.msize[ 


ysize = map.msizef 1]; 
zSize = map.msizef 2] ; 
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ee 


343 
344 
345 
346 
347 
348 
349 
350 
351 
352 
353 
354 
355 
356 
357 
358 
359 
360 
361 
362 
363 
364 
365 
366 
367 
368 
369 
370 
371 
372 
373 
374 
375 
376 
377 
378 
379 
380 
381 
382 
383 


385 
386 
387 
388 
389 
390 
391 
2 
B03 
394 
395 
396 
Bo / 
398 
399 
400 


z = (int) ((SONAR HEIGHT + HEIGHT OFFSET - map.lomvj 2]) / 
(map.himvji 2] - map.lomv{ 2] ) * zsize); 


iri ede ae; 

for (y = 0; yy < vsaze; yer — ERIN s7BE) 4 

for {<= 0; x =< xsize, x t= PRINT SiEP) 4 

if (yaxis == 1) { 

b= Map mapm z* xSize * ySize=rolysize — y = 1) * “eZee x; 
} 

else { 

Pp = Map.mapmi z * xsize * ysize + y * xsize + x] ; 
} 

ie et) 

Oriner tts Wy 

} 

else if (p == 0) { 

Prine 2”); 

} 

else if (p> -25) { 

PrintL{’ <)> 

} 

else if (p > -50) { 


PEeunerm = 4): 
} 

else { 

fone ieee ree 


} 
} 
Prince s\n"): 
} 
} 


void laser print (Map3D map, int yaxis) 
> 
Print evidence grid occupancy probabilities for sonar level 


mo 
{ 


inves <5 y, 2: /* Cell index */ 
int xsize, ysize, zsize; /* Grid dimensions (# cells) */ 
aca: /* OCCupency Drobability > 7 
int empty; /* Empty level flag */ 
xSsize = map.msizef 0] ; 
ysize = map.msizef 1]; 
zsize = map.msizel 2] ; 
Ze Ie oP (EASRR ONE Gi EGO r hor lee tia. VOmy cals) soy 
(map.himv[ 2} - map.lomv[ 2] ) * zsize); 
DEiIne ee ye 
EOL S(Ve= OU; eVUSrysSize,; @y0 = eee) eo me) o 
Or BAe a0 ae Se ext PRINT OER) a 
if (yaxis == 1) { 
p = map.mapm[ z * xsize * ysize + (ysize - y - 1) * xsize + x]; 
} 
else { 


py] Map .mapnl z -* xsize * -ysize + y ~*~ -xsize + x]; 
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401 } 


402 ie iors 0 yt 

403 prince do, 

404 } 

405 else if (p == 0) { 

406 primer 2 

407 } 

408 else if (p > -25) | 

409 Deloer( sy 

410 } 

411 else 18 (pp >9=50) 4 

412 pints 2 e 

413 } 

414 ese 

415 penta 

416 } 

417 } 

418 Prine \ ae: 

419 } 

420} 

421 

422 void grid display (Map3D map, {> Evidence Grad 7 

423 7 double height, /* z-coord of plane to display *7 
424 ints origin, /* World x-coord of origin (1/10 inch)*/ 
oP iit yoeoragih)) /* World y-coord of origin (1/10 inch)*/ 
4 /* 

427 Display evidence grid occupancy probabilities in robot window 
428 + / 

429 { 

430 double xd, yd; /* Display coords */ 

43] double xscale, yscale, zscale; /* Cell dimensions (tenths of 
432 inches) */ 

433 double xoffset, yoffset; /* Circle center offset */ 
434 Pit, /* Cell index / 

435 int “size, yelze, 2oize: /* Grid dimensions (# cells) */ 
436 int p; /* Occupancy probability */ 

4377 int empty; /* Empty level flag */ 

eS /* aint ead: / /* Radius of cell display */ 
43 

aay primntt ("Displaying grid at (2d,.2d)\m'y x origin, y-or1¢sn), 
442 xsize = map.msizef 0] ; 

443 ysize = map.msize{ 1] ; 

dda zsize = map.msize{ 2] ; 

445 

446 xscale = (map.himv{ 0) - map.lomv{ 0) ) * 120.0 / (double) xsize; 
4477 yscale = (map.himv{ 1] - map.lomv{ 1})) * 120.0 / (double) ysize; 
He zscale = (map.himv[ 2] - map.lomv[ 2]) * 120.0 / (double) zsize; 
450 “Ofiset = Mscale / 2.0; 

451 yOorfset = yscale / 2.0; 

452 

453 z= (int) ((height + HEIGHT OFFSET - map.lomv{ 2]) / 

454 (map.-himv{ 2] - map.lomv[ 2]) * zsize); 

455 

456 for (y= 074 = ysize; yr) 4 

457 for (x = 0; x < xsize; x+t+) { 

458 Dp = map.mapm[ z * xsize * ysize + y * xsize + x]; 
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459 
460 
461 
462 
463 
464 
465 
466 
467 
468 
469 
470 
47] 
472 
473 
474 
475 
476 
477 
478 
479 
480 
481 
482 
483 
484 
485 
486 
487 
488 
489 
490 
491 
492 
493 
494 
495 
496 
497 
498 
499 
500 
501 
502 
503 
504 
505 
506 
507 
508 
509 
510 
511 
B12 
513 
514 
15 
516 


“due (int) ((double) «x * xscale + map.lomy 0) * 120.0) “+ x Vorigan, 
yae= (ant) ((deuble) y ~ yecale + map.tomy 1) * 120.0) + yiorigin; 
/* rad = (int) (((double) (p - NEG) / (double) POS) * (double) 


“scale = O25); 
draw arc (xd, ya,--rad, rad, 0; msc00 7 i) 77 / 


SSE a0 et 
draw _arc(xd, yd, xscale, yscale, 0, 3600, 1); 

} 

else if (p == 0) { 
draw ere(xd, yd, xscale 7 4.0, xscale / 4.0, 0, 3600; 1); 

} 

} 
} 
} 


void grid display pos(Map3D map, /* Evidence grid */ 
double height) j* Z-coord of plane to display: 

/* 

Display evidence grid occupancy probabilities in robot window at 
Deslt ion 
kr: 
1 

mice Ox, Ay, 


printf ("Enter display coordinates ==>"); 
scanf(" %d %d", &dx, &dy); 


grid display(map, height, dx, dy); 
} 


void model init (CylSensorModelArray *sonar_smd, 
CylSensorModelArray *sonar_clear_smd) 
/* 
Initialize sensor models 
ey 
{ 
InitCylModelParams(); 


MakeCylModel (66, 0.02, 64, 128, 1.0, 22.0, sonar_smd); 
TrimCylModel (* sonar smd) ; 


/* WriteCyiModel (* sonar smd, "sonar.mod” ) ;* / 
/* ReadCylModel ("sonar.mod”, sonar smd) ;* / 


MakeClearCylModel (66, 0.02, 64, 128, 1.0, 22.0, sonar _clear_smd); 
Ty imey Modeli= sonar teleacr csmd); 


/* WriteCylModel (*sonar_clear_smd, "clear.mod") ;*/ 
/* ReadCylModel ("clear.mod", sonar _clear_smd) ;*/ 


} 


void sonar_scan(CylSensorModelArray smd, CylSensorModelArray clear_smd, 
Maps) map, ant Gx, Ant Fy, ant rtheca) 
/* 
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518 
519 
520 
52 | 
22 
523 
524 
2) 
526 
a7] 
528 
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530 
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559 
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542 
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547 
548 
549 
550 
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5D5 
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70 
574 


Update evidence grid using all sonar sensors 


a a 
{ 

PosData sonar _posef{ 16] ; /* Sonar pose information */ 

double robot_x, robot y; [* RObOE POsTELOm 77 

double robot theta; /* Robot: heacdrag = / 

double range; /* Range reading (feet) */ 

double angle; /* Sensor angle (radians) */ 

double sonar_pos[ 3] ; /* ‘Soner position 7 / 

double sonar _dir{ 3] ; /* Sonar Civeceion 7 

int reading; /* Raw sonar reading */ 

abate /* sonar index / 

gs(); /* SCOUT THESIS CHANGE use gs to get sonar and position info */ 
/* posSonarRingGet (sonar pose); SCOUT THESIS CHANGE - comment this 


Meine 2Onte) 4/ 
/* SCOUT does not currently provide pose data as NOMAD 200 does */ 


for (1 = 0. 4° < Gi 
/* SCOUT THESIS CHANCE 
comment out the requests for pose information below 


robot _ x = (double) sonar_posef[ i] .config.configxX / 120.0; commented out 
robot y = (double) sonar pose i] .config.configY / 120.0; commented @ene 
robot theta — (double) sonar posé/ i) - config. contig@turrec/ =10. 0, 
commented out 

oy 


/* SCOUT THESIS CHANGE uncomment out the lines below 
and get the sonar data from using the gs command */ 
robot x = (double) rx / 120.0; 
wobot y = (double) zy 7 120720; 
robot theta = (double) rtheta / 10.0; 


reading = State{[i + 17]; 
range = (double) reading / 12.0; 
( 


angle = ((double) 1%* SONAR SEP + robot_theta) * M DEG2RAD; 
sonar dixzf 0] = cos(angle); 

sonar diz{ 1) = sin(angle); 

sonar dir, 2] = U.0; 


sonar pos{ 0] = sonar _dir{ 0] * SONAR_RAD; 
sonar _pos{ 1] = sonar_dir{ 1] * SONAR_RAD; 
sonar pos{ 2] = SONAR HEIGHT + HEIGHT OFFSET; 


if ((reading != MAX SONAR READING) && (range <= MAX SONAR RANGE) ){ 
AddCylReading(range, sonar _pos, sonar dir, smd, map); 
} 
else { 
AddCylReading (MAX SONAR RANGE, sonar pos, sonar dir, clear_smd, 
map) ; 
} 
} 
} 


void sonar scan_abs(CylSensorModelArray smd, CylSensorModelArray 
clear smd, 
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595 
596 
Do / 
598 
99 
600 
601 
602 
603 
604 
605 
606 
607 
608 
609 
610 
611 
612 
613 
614 
615 
616 
617 
618 
619 
620 
621 
622 
623 
624 
625 
626 
627 
628 
629 
630 
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632 


Map3D map, int rx, int ry, int rtheta) 
/* 
Update evidence grid using all sonar sensors (using absolute position) 


7 
{ 


PosData sonar_pose[ 16] ; /* Sonar pose information */ 
double robot _x, robot_y; /* (ROSOE. POST eion —/ 

double robot_theta; /* Robot heading */ 

double range; /* Range reading (feet) */ 
double angle; /* Sensor angle (radians) */ 
double sonar_pos[ 3] ; /* Sonar postion = / 

double sonar _dir[ 3]; /* “Semar direction */ 

int reading; /* Raw sonar reading */ 

ane 1; /* Sonar index. */ 


gs(); 
/* posSonarRingGet (sonar pose); SCOUT THESIS CHANGE - comment this 


line out */ 
/* SCOUT does not currently provide for pose data as the NOMAD 200 does 


Pa. 


ner (i= 0; oi =< 16; 1s) 4 


Vo robot x = (double) sonar _posef{ i] .config.configxX / 120.0; ** 
comment this line out */ 

yes robot y = (double) sonar pose i] .config.configy / 120.0; =e 
comment out */ 

i robot theta = (double) sonar_pose[ i] .config.configTurret / 10.0; 


** comment out */ 


/* uncomment out the lines below and use gs command to get sonar data 
ay 

EODOE *~ = (double) «ux 7 120.0; 

robot_y = (double) ry / 120.0; 

robot theta = (double) rtheta / 10.0; 


reading = State{[i + 17]; 
range = (double) reading / 12.0; 
( 


angle = ((double) 2 ~ SONAR She + roboc theta) = M DEGZkAp, 
sendy di 0 9— cos(angle), 

Sonar dir 1) = Sintanghe); 

Sonar eadir[s2| = 0.0; 


sonar _posf 0] sonar _dir{ 0} * SONAR RAD + robot_x; 
sonar pos 1] sonar dir |) + CONARSRAD + roberly, 
sonar pos[ 2] = SONAR HEIGHT + HEIGHT OFFSET; 


if ((reading != MAX SONAR READING) && (range <= MAX SONAR RANGE) ){ 
AddCylReading(range, sonar_pos, sonar dir, smd, map); 
} 
else { 
AddCylReading (MAX SONAR RANGE, sonar _pos, sonar _dir, clear_smd, 
map) ; 
} 
} 
} 
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/* BEGIN SCOUT CHANGE */ 

/* NOTE - it appears that the following function is never used by any 
exploration routine */ 

/* Left in code for now since it will not affect the Scout */ 

/* The header for this is in grid++.h */ 


void ir _scan_abs(CylSensorModelArray smd, CylSensorModelArray clear smd, 
Map3D map, ant ©x, Intry, int reheta) 


/* 
Update evidence grid using all infrared sensors (using absolute 
position) 
ee 
{ 
PosData ir pose{ 16] ; /* TRepese imftormeation ~*~ / 
double robot_x, robot_y; /{RODOE POSEELONs: 7 
double robot. theta; /* Robot heading */ 
double range; /* Range reading (feet) */ 
double angle; /* Sensor angle (radians) */ 
double ir pos[ 3]; j* iB ees. bicn =7 
double “ar <ai7j73)) 7, /> AR Gurection=*/ 
int reading; /* Raw IR reading */ 
Pe 5 /* SOnar Andex =/ 
gs(); 


pos ImiravedRingGcer (i spose)”, 


BO ole le G1 liek ye 
PODOt sk = {double wit spose 1) contig. contra, /-120.0, 
LODOE yy — (Gouble) Mir spose) 4).- contig, contvg: / 120-0, 
robot. theta = (double) ir pose 1). contig.contig@iurrer 7 10207 
i PObOt «.—. (double) ex 7 120.0; 
robot y = (double) ry / 120.0; 
robot theta = (double) rihneta/ 120 .0;*7 
reading Statef[ i+ 17]; 


range = (double) reading / 12.0; 
ou 


angle = double) “i “9 Iheskr + robot ,enetaj-- M PEGZRAD, 
tbe Gir 0] = cos(angle); 
Leiria |) = Siniang te); 


ie Pda 2) = 0-07 


if pos! 0] = ir deri 0) * eeeaba robot rx, 
ir posi 1] i Gas ly) = Renee nope ty; 
it pos 2) = TR EIGHT + HEGCHT OFPSET; 


if (reading < MAX IR_READING) { 
RddCyl Reading (zvange, 1r pos, inidix, smd, map); 
} 
} 
} 


/* END SCOUT CHANGE */ 


void sonar _scan_abs_norep(CylSensorModelArray smd, 
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CylSensorModelArray clear smd, 
MapsD map, Int rx; ane ry, int rtheta) 
[* 


Update evidence grid using all sonar sensors (using absolute position) 


(no updates for repeated positions) 


ea 
{ 
Seaere Long vold=xiekol, Old vidlGi. 7 sold robot positicn =/ 
Seatic int first flag = 1; \/* Reset first’ time function is called-* 7 
PosData sonar_pose[{ 16) ; /* Sonar pose information */ 
double robot _x, robot y; /< “Robot (position 7 
double delta; /* Change in robot position since last 
update */ 
double robot theta; /* Robot heading * / 
double range; /* Range reading (feet) */ 
double angle; /* Sensor angle (radians) */ 
double sonar _pos[ 3] ; 7 S5Onar posteton */ 
double sonar dir{ 3]; jmMesonat direction. */ 
int reading; /* Raw sonar reading */ 
lee a7 /* “sonar index ~/ 
gs(); 


posSonarRingGet (sonar pose); 


Pome le— OF 22m Noe itty 4 
BOOCE x —)( double) sonar pesc| 4) weoneigq.configk, 7 120.0; 
Popct yy — (double) »sondn pesel a contig. contigy / 120.0, 
mobos theta = Kdeuble) sonar posc[ i) scontig.contiglurretrs, 20507 


delitai= hypoe (| Goupte me somar ese fT) contig. contigs. oO) Clete 
(double) “sonar pose! 1] 2conticgscontigy = old_yiil)); 


if (first_flag || delta >= MIN DELTA) { 


old xi) = Sonar pose aj conrig.contigx; 

Sold yi1) = Sena pose! a] contig .contigy; 

reading = State[i + 17]; 

range —*(deuble) -readung «/ 12.0; 

ange -—=—((doubte) 1 ~ SONAR ESEP s+ robot (Reta ma MeDEGZnae- 
sonar dir{ 0] = cos(angle); 

sonar _dir{[ 1] = sin(angle); 


sonar dir{ 2] = 0.0; 


sonar_pos[ 0] = sonar _dir[{ 0] * SONAR_RAD + robot x; 
sonar pes! 1) = sonar dir 1] 7 SONARGRAD 4 robot y;? 
sonar _posl 2] = SONAR HEIGHT + HEIGHT OFFSET; 


if ((reading != MAX SONAR_READING) && (range <= MAX SONAR RANGE) ){ 


AddCylReading(range, sonar_pos, sonar dir, smd, map); 

} 

else { 

AddCyl Reading (MAX _SONAR_RANGE, sonar_pos, sonar dir, clear_smd, 
map) ; 


} 


i else { 
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769 
770 
dl 
fe 
iS 
774 
TT 
776 
Wee? 
778 
qa9 
780 
781 
782 
783 
784 
785 
786 
787 
788 
789 
790 
791 
2 
193 
794 
195 
796 
7197 
798 
199 
800 
801 
802 
803 
804 
805 
806 


Prints( Sonar scan abs morep: Kepeated pos itien. (sd; td) ter 
sensor #d.\n", 
Sleox iy, ole yi i) 29% 
7 
} 


first t24¢ = 07 
} 


void laser update (Map3D map, double rx, double ry, double 1x, double ly, 


double rtheta) 


/* 
Update evidence grid for a single laser reading 

a 

{ 
double ir, ltheta; /* Laser vector */ 
double wx, wy; /* World coords of laser endpoint */ 
double xsize, ysize; /* “Size Of Grid cella 
double stepsize; /* Stepsize along laser axis */ 
double dx, dy; /* Stepsize along x and y axes */ 
double px, py; /* Point currently being updated */ 
int steps; /* Number of steps */ 
ae ele, 


ir = hypott(ix, iy); 
ltheta = atan2(ly, 1x) * M _RAD2DEG; 


rx + lr * cos((ltheta + rtheta) * M DEG2RAD); 
ry + lr * sin((ltheta + rtheta) * M DEG2RAD); 


wx 
wy 


set _location(map, wx, wy, LASER _HEIGHT, POS); 


px = x; 
Py = fry; 


xsize = (map.himv[ 0] - map.lomvj 0]) / map-.msize{ 0] ; 
ysize = (map.himv[ 1] - map.lomv{ 1] ) / map.msize{ 1) ; 


if (xeize < vsize) 4 
stepsize = xsize; 
} 
else { 
stepsize = ysize; 


} 


dx = stepsize™ cos((ltheta —) reneta) * M DECZRAD); 
Gy = stepsize * sin((ltheta = reheta) ~ MUDECZRAD); 
steps = (int) (lr / stepsize); 

for (1 = 03 2 < steps; i447) { 


set _location(map, px, py, LASER HEIGHT, NEG); 
px += dx; 
py t= oy 
} 
} 


void laser scan(Map3D map, int ©xX, InNeery. ome Teneca) 
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807 = /* 


808 Update evidence grid using laser scanner 
809 * / 
810s { 
811 PosData laser pose; /* Laser pose information */ 
812 double 1x, ly, lr, ltheta; /* Laser point (robot coordinates) */ 
813 double wx, wy, WZ; /* Laser point (world coordinates) */ 
814 double robot_x, robot_y, robot_theta;  /* Robot location */ 
815 ise 15; 
816 
817 gs ()> 
nS posLaserGet (&laser_ pose); 
8) 
820 robot _x = (double) laser pose.config.configxX / 120.0; 
821 robot _y = (double) laser pose.config.configY / 120.0; 
822 robot theta = (double) laser _pose.config.configTurret / 10.0; 
823 
824 for (i = 0; i < Laser{ 0); itt) { 
825 /* printf("| ad, td) "| Laser[.1 * 2 + 1), Lasen i ~*~ 2 + 2) )577 
826 if (Laser[i * 2 + 1] != 65000) { 
827 lx = (double) Laser[i * 2 + 1] / 120.0; 
828 ly = (double) Laser[{i * 2 + 2] / 120.0; 
829 Vf DEIMER( Vet, el)! bs, 2 ye 
830 
=> Ve laser update(map, robot_x, robot_y, 1x, ly, robot theta) ;*/ 
8 
833 lr = hypot(lx,.1ly); 
es ltheta = atan2(ly, 1x) * M_RAD2DEG; 
> 
836 if (lr <= MAX LASER RANGE) { 
837 wk — Jr * cos((ltheta + robot theta) ~ M DEGZKAD); 
838 wy = lr * sin((ltheta + robot_theta) * M DEG2RAD); 
839 wz = LASER HEIGHT + HEIGHT OFFSET; 
840 set_location(map, wx, wy, wz, POS); 
841 is drawsdine((imt) robot x * 120.0, dant) rebot,. 
mt) 120.0, (int) wx * 120.0, 
843 (int) wy * 120.0, 19)-*/ 
844 } 
845 } 
846 = /* Prints own" ) 7 
847 } 
848} 
849 


850 vOld laser scan abs (MapoD map, imtorx, int ry; Int veneca) 


el 86 /* 


852 Update evidence grid using laser scanner (using absolute position) 
853 * / 

854 { 

855 PosData laser pose; /* Laser pose information */ 

856 double Ixy, ly; ir, -ltheta; o/** Laser point. (robert coordinates). ~/ 
857 double wx, wy, wzZ; /* Laser point (world coordinates) */ 
858 double robot x, robot y, robot theta; /* Robot location * / 

859 Tee 7 7 7 

860 

861 gsiig 

862 posLaserGet (&laser pose); 

863 = 

864 TOD@Le x = (double) taser pOse, cont. Contlgn 4/7 420.0; 
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robot y = (Gdouble) (laser (pcse- contig. contig) 7512020; 
robot seneta = (double) laser pose contig. contig arrcr 7 sed, 


for (1 = 0; i.< Laser 0); is) { 
/* printii"| sa, sd) ", Laseri 1 * 2 + 1], Laser 1°" >2 2 Zi eee 
if (Laserf{i* 2 + 1] != 65000) { 
* 
* 


1x = (double) Laser[ i 2 te, 120.0; 
ly = (double) Laser[ i Ae 2a 1200 
fo DiC C al ata oh ye ey ase, 
/* laser update(map, robot x, robot _y, 1x,> ly, robotmencea i, 7 


ie = bypoe (Ex Fy) 
ltheta = atanzZ(ly, 1x) * M RAD2ZDEG; 


if (lr <= MAX LASER RANGE) { 
= Tp "eos ((beneta + robot stnetaj=- ia PeeZnnbi rODOt Ra, 
wy = Ilr * sin((ltheta + robot theta) ~ M DEG2RAD) + robormy 
wz = LASER HEIGHT + HEIGHT OFFSET; 
set_location(map, wx, wy, wz, POS); 
Te draw line((int) robot_x ™ 120.0, (int) seboumma 
ZOO aCe wee Oe 
(Ine) wy L200, Lo) 7 
} 
} 
j= Deine oy 
} 
} 


double laser min(void) 


7% 
Return minimum laser range reading 
a 
double min_range = MAX LASER_RANGE; /* Minimum range reading 77 
double 1x, ly, 1, eneca, /* Laser point (robot coordinates) 
ay 
Seto eels 
gs(); 


for (i = 0; i < Laser[ O} ; itt) { 
if (Laser{i* 2 + 1) != 65000) { 
de (double) Laserf{i* 2 + 1} / 120.0; 
ly (double) Laser[{i* 2 + 2] / 120.0; 


Ht ll 


in =mypot (lx, ly) 


if Lae mem enge) 4 
min range = lr; 
j 
} 
} 


return (min_range) ; 


} 


void lls scan(CylSensorModelArray smd, CylSensorModelArray clear_smd, 
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964 
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7 = 


a7 


{ 


oy 


/* 


MapsD Map, Int rx, 1ne ry eant roheca) 


Update evidence grid using laser-limited sonar 

PosData sonar pose[ 16] ; /* Sonar pose information */ 

PosData ir pose[ 16] ; j/~ IR pose iIntormat ron = / 

PosData laser _ pose; /* Laser pose information */ 

double sonar_x, sonar _y; /* Sone PosteLon * / 

double sonar theta; /* Sonar angle */ 

double laser_x, laser y; jj  Laset, POSTON ~ 7 

double laser theta; /* Laser angle */ 

double lx, ly, lr, ltheta; /* Laser point (robot coordinates) */ 
double wx, wy, WZ; /* Laser point (world coordinates) */ 
double min_laser_range = MAX LASER_RANGE; /* Minimum laser reading 
double sonar range; /* Range reading (feet) */ 

double angle; /* Sensor angle (radians) */ 

double sonar pos 3] ; /* “Sonar *pOsibien. = / 

double sonar dirf 3]; }* “SOWMaAG  GlEVveecelor. = 7/ 

double angle diff; /* Angle offset between laser and sonar */ 
int reading; /* Raw sonar reading */ 

tai > 1. 


/* Get sensor and pose data from robot */ 


gs(); 
posSonarRingGet (sonar _ pose); 
BesinitraredRingGet (ir pose); 
posLaserGet (&laser pose); 


/* Update grid using laser readings */ 


lidecme xe. (Couble) laser pose. contig contigx. / 120.0; 
iaseray — (doub¥e) laser pose.contag.consed:, / 120-0; 
laser theta = (double) laser pose.config.configTurret / 10.0; 


for tie= O;-“a < Laser 0] ; att) { 


Ux Prince | sa, sd] ">7 basen 7 -2 5 alley Laser ote) 
if (Laser[i* 2 + 1] '= 65000) { 
lx = (double) Laser[i* 2 + 1] / 120.0; 
ly ="(deuble)tLaser a * 27472] e771 120.0; 
75 PLEIN eE Cle Le Seb XG PLY 


fase reupdace (map, #laserex; (laser yy, 3 lx, oly, laseucencea) sy 


le=="hypoe (ls. Ly); 
tee eee eM Ii ase pe trange) a, 
Mitwlaser =nange=— 12; 


} 
TENGGay= atanz (ly, 71x) >"> MaRADZDEG, 


if (lr <= MAX LASER RANGE) { 
Wx =o lr *cos(({lthera + laser thera))~ MepEGZRAD):;, 
wy = lr * sin((ltheta + laser Beneta) ~  M PEGZRAD) 7; 
W2 EP SER CHE GH) <+ Hel GH) OPPSET,; 


les 
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1038 


set _location(map, wx, wy, wz, POS); 
/* draw  dine( (int) lasérex * £2020 3 (ing) @baccrun, | 


t20,.0,5 {ant) we * sie, 


/* 


(int) wy * 120.0, 19);*/ 
} 
} 
Print en 
} 


/* Update grid using sonar reading (limited by minimum laser range) */ 


sonar x = (double) sonar pose| 0) -coniig-configx 7 120.0; 
sonar _y = (double) sonar_pose[ 0] .config.configY / 120.0; 
sonar theta =— (double) sonar pose 0] contig. contigiurver 7 sige. 


reading = State[{ 17); 
/* At very close ranges, use infrared instead */ 


/* if (State[ 1] < MAX IR READING) { 
reading = State 1]; 


sonar x = (double) ir_pose[ 0] .config.configxX / 120.0; 
sonar y = (double) ir pose 0) .config.configy / 120.0; 
sonar theta = (double) ir _pose{ 0) .config.configTurret / 10.0; 


printf ("laser/IR offset = @f inches : %f degrees\n", 
hypot (sonar_x - laser _x, sonar_y - laser y), 
Senmar .bneta, = Laser tueva) 
} 
else { 
printf ("laser/sonar offset = %f inches : $f degrees\n", 
hypot (senar «x ~slaser ix,., sonar sy = laser sy); 
sonar theta - laser theta); 
jn 
/* Compute angle offset between laser and sonar (or IR) */ 
angle diff = fabs(sonar theta - laser theta); 
Le (angle ditt = 280.0) { 
anglevdi hE = S00, 0=> angle dit. 
} 
/* Discard reading if offset is too large */ 
if (angle diff > LLS MAX ANGLE DIFF) { 
printf("LLS reading discarded: angle offset = %f\n", angle diff); 


reriwrn- 


} 

/* Determine LLS range */ 

sonar range = (double) reading 7 12.0, 
1£ (semar@range> min laser range) 4 


sonar range = min_laser range; 


} 
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J Upgate grid */ 


angle = sonar_theta * M DEG2RAD; 


sonar 7air| 0] = cos (angie) ; 
B@mar oie!) = sim (angie), 
sonar dir{ 2) = 0.0; 


sonar pos 0] sonar dir{ 0] * SONAR_RAD; 
sonar pos 1] sonar dirf 1} * SONAR RAD; 
sonar pos[ 2] = SONAR_HEIGHT + HEIGHT OFFSET; 


il 


I 


if ((reading != MAX SONAR READING) && (sonar_range <= 
MAX SONAR_RANGE) ){ 
if (sonar_range <= MAX SONAR _OCC_RANGE) { 
AddCylReading(sonar_range, sonar_pos, sonar_dir, smd, map); 
} 
else4 
AddCylReading(sonar_range, sonar_pos, sonar dir, clear_smd, map); 
} 
} 


else {= 
AddCylReading (MAX SONAR_RANGE, sonar_pos, sonar_dir, clear_smd, 
map) ; 
} 
} 


voc ils scan abs (CylsensorModelArray smd, CylsensorModelérray 
clear smd, 
Map3D map, int rx, int ry, int rtheta) 
/* 
Update evidence grid using laser-limited sonar (absolute coordinates) 
= 


{ 


PosData sonar _pose[{ 16] ; /* SOvar pose information ~ 7 

PosData ir pose[{ 16] ; /* IR pose information * / 

PosData laser pose; /* Laser pose information */ 

double sonar_x, sonar_y; /* Sonar position */ 

double sonar_theta; J sOner angie ~/ 

double laser x, laser y; /* Laser position */ 

Goeuble laser theta; /* Laser angle *~/ 

double Ix, ly, ir, ltheta; /* Laser point (robot coordinates) 77 

double wx, wy, wzZ; /* Laser point (world coordinates) */ 

double min_laser_range = MAX LASER RANGE; 7 Minimums lasere7 eacking 
Pe 

double sonar range; /* Range reading (feet) */ 

double angle; /* Sensor angle (radians) */ 

double sonar_pos[ 3] ; /* esOnas Postelons 7 

double sonar dir{ 3]; /* Sonar direction */ 

double =angierdi it; /* Angle offset between laser and sonar */ 

int reading; /* Raw sonar reading */ 

ae aes 


/* Get sensor and pose data from robot */ 


gs(); 
posSonarRingGet (sonar _ pose) ; 


Sy, 


Pm fe eh eh pee et pet pet pt 
eh pre fre pam fem eh fem fem meh fee ed 
WORD i i 
m= OOOO SIAN BWN 


— 
— 
NWN 
WhO 
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id 
LZ 
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1154 


fx 


/* 


posInfraredRingGet (ir pose) ; 
posLaserGet (&laser pose) ; 


/* Update grid using laser readings */ 


laser x = (double) laser pose.config.configX / 120.0; 
laser y = (double) laser pose.config.configy / 120.0; 
laser theta = (double) laser pose.config.configTurret / 10.0; 
for (i = 0; i < Laser[ OJ ; i++) { 
/* printf ("[ $d, td) ", Laser[ 1 * 2+ 1], Laser[a * 2 + Zi 
if (Laser{i * 2 + 1) != 65000) { 


lx = (double) Laser[i* 2 + 1) / 120.0; 
ly = (double) Laser[i* 2 + 2] / 120.0; 
7. Dr rat (a Ree sae) ee Se a era 


Taser update(map, laser x, laser y, 1x, ly, .lasergenetai sy 


lr = hypot(lx, 1 
if Sie mMinvlasem range, *{ 
min laser range = lr; 


j 
ltheta = atan2(ly, 1x) * M RAD2DEG; 


if (lr <= MAX LASER RANGE) { 
wx = lr * cCos((ithera + laser theta) * M DEG2RAD) + lascuiee 
wy = lr * sin((ltheta + laser theta) * M DEG2RAD) + lasers 
w2-= LASERSHE TGR + HETGRT OPronl, 
set location(map, wx, wy, wz, POS); 
i draw line((int) laser x * 120.0, (int) laser 


L260 0. Mane) ew See Ze 


(ine ewy). t20.050 Lo) e7 
} 
} 
pie tee Or a 
} 


/* Update grid using sonar reading (limited by minimum laser range) */ 


Sonar x = (double) sonar pose Uj. cont lg conkign 7 erZ0n0, 
sonar y = (double) sonar posef 0] .config.configy / 120.0; 
sonar theta = (double) sonar pose 0) .config.configTurret / 10.0; 


reading = State{ 17]; 
/* At very close ranges, use infrared instead */ 


/* if (State{ 1] < MAX IR READING) { 
reading = State[ 1]; 


sonar x = (double) ir pose[ 0] .config.configX / 120.0; 
sonar _y = (double) ir _pose[ 0] .config.configy / 120.0; 
Sonar theta = (double) iy pose Ol contig-coniigluncere, 10.0, 


printf("IR/sonar offset = %f inches : %f degrees\n", 
hypot(sonar x = laser x, ‘sonar y > Laser vy). 
sonar theta — laser theta); 
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} 


else { 

printf("laser/sonar offset = $f inches : sf degrees\n", 
hypot(sener * = laser x, Sonar y = taser yy), 

sonar theta = laser theta), 

poe 


/* Compute angle offset between laser and sonar (or IR) */ 


angle diff = fabs(sonar_theta - laser theta); 
1: (angle vd tf > 130.0) 4 

angle diff = 360.0 - angle diff; 
} 


/* Discard reading if offset is too large */ 
if (angle diff > LLS MAX ANGLE DIFF) { 
DEtntit Lis reading Giscarded: vangle -Oftset.—= Sf\n', angle Grrr); 


return; 


} 

/* Determine LLS range */ 

sonar range = (double) reading / 12.0; 

if (sonar_range > min_laser_ range) { 
sonar range = min_laser_ range; 

} 

/* Update grid */ 

angle = sonar_theta * M DEG2RAD; 

cos(angle); 


Sin(angle); 
O20; 


sonar_dir[ 0] 
sonar dirf[ 1) 
Sonar sdi xr 2] 


i | 


sonar pos[ 0] 
Somer sposi !) 
sonar pos[ 2] 


sooner dim 0) ~*~ SONAR RAD + Sonar ux; 
sonar_dir{ 1] * SONAR RAD + sonar_y; 
SONAR _HETGHT + HEIGHT OFFSET; 


if ((reading != MAX SONAR READING) && (sonar_range <= 


MAX SONAR_RANGE) ){ 


if (sonar_range <= MAX SONAR OCC RANGE) { 
AddCylReading(sonar_ range, sonar pos, sonar dir, smd, map); 
} 
else { 
AddCylReading(sonar range, sonar _pos, sonar _dir, clear_smd, map); 
} 
} 
else: 
AddCy1lReading (MAX_SONAR_RANGE, sonar pos, sonar dir, clear smd, 


map); 


} 


void clear robot (Map3D map, int rx, int ry) 
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/* Set grid cells under robot to be unoccupied */ 


double wx, wy; /* Robot location (world/feet) */ 

int Ix, ly, .hZeex enn /* Corners of robot soounding 
box 27 

int Cx, Cy perec, /* Grid cell coordinates */ 

wx (double) 1x 7 420.0; 


wy (double)i my~/ 3220.0; 
if (world2grid(map, wx - ROBOT RADIUS, wy - ROBOT RADIUS, 0.0, 
als, Sly piel z) == —1) { 
printf ("clear robot: robot edge (%£, %£, sf) out cf range. wa 
wx - ROBOT RADIUS, wy - ROBOT RADIUS, 0.0); 
return; 
} 
if (world2grid(map, wx + ROBOT RADIUS, wy + ROBOT RADIUS, 
ROBOT HEIGHT, 
&hx, &hy, &hz) == -1) { 
printi(*elear robct. robet edge (ti, ti, sf) Out Of mange. wa 
wx + ROBOT RADIUS, wy + ROBOT RADIUS, ROBOT HEIGHT) ; 
return; 


} 


for (cx = 1x) 1exec]— hx; cx) { 
for (cy = ly; cy <= hy; cy+t+) { 
fOr. (CZ l=sl2 7 lez, @— Nas, Cz2t a 
Set Grid(Map, cx, cy, C2, NEG); 


} 


} 


Void Grid clear (Map3P grid) 
/* 
Clear Current grid; 
oy, 
{ 
ClearMap3D(grid); 
} 


void grid decay(Map3D grid) 
J* 
Decay grid cells towards base probability 
* dh 
{ 
ie. 1, ene 


km = 
1<< (ILOG2C (grid.msize[{ 0} )+ILOG2C (grid.msize[{ 1] )+ILOG2C (grid.msizef 2] )); 


for (k=0; k<km; k++) { 


if (grid.mapm{ k} != Q) { 
if (grid.mapn{ k} > GRID DECAY) { 
grid.mapm{ k] -= GRID DECAY; 


} 
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else if (grid.mapn[ k] < -GRID DECAY) { 
grid.mapn{ k] += GRID DECAY; 

} 

else { 
grid.mapm{ k]) = 0; 

} 


} 
woud Grid translate (MapsD gridl, Map3D grid2, double dx, double dy) 


/* 
Translate all cells in <gridl> by <dx, dy> (feet) and store results in 


—GeLd2> 
=] 
{ 
double wx, wy, wzZ; /* World coords */ 
Mite ot ees /* Ioieial grid cell coordinates + / 
int trans_index; /* Transformed cel index. 9/7 


Preiner( Translating by (ert, Sf) vay dx, dy); 


pueiner ("Initial geld yn’); 

grid display(gridl, SONAR_HEIGHT, 0.0, 0.0); 
PEIneh (Hit <return> nN); 

getchar(); 


Grid clear( grid? |; 


for (x= OF x < Gridl-msize 0]; x++) 4 
FOr (y = 0; y = Oieidil_msazel ip ytr+) { 
£Oe (2 = "O72 —agridlsmsuze, Zin 2t4). 4 
GCrvaZworlad(Grial,. x, “VY, 2) awe; Guy, Gw2); 
trans index = world2index(gridl, wx + dx, wy + dy, wz); 
if (Glans index (= ‘=a 
grid2.mapm{ trans index] = 
Gridi..mapm gridzindex (Graal, x,y, <2) ).- 


} 
} 


Printer Transbated Grid) ; 
grid display(grid2, SONAR_HEIGHT, 0.0, 0.0); 
Ouimue( Hiee<recurn> (nm); 
getchar(); 
} 


VOUCGr Ce rOraselMapsDeguial .. Mapsbegrid] taoublesdinera 
/* 

Translate all cells in <gridl> by <dtheta> (degrees) and store results 
im 


<GE2a2c> 
rue 
{ 
double wx, wy, wzZ; J* Cartesian. world “coords Of anit1a point 
a) 
double rx, ery; /* Rotated Cartesian coords */ 


fo 
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1359 
1360 
1361 
1362 
1363 
1364 
1365 
1366 
1367 
1368 
1369 
1370 
1371 
1372 
Noe3 
1374 
1375 
1376 
ey 
1378 
179 
1380 
1381 
1382 
1383 
1384 
1385 
1386 


ey 


double dx, dy; /* Cartesian vector from center to point 
double radtheta; /* Rotation in radians */ 

double r, theta; J/* Polar coords from Center torpoint.~7 
ee XG oe /*" tpitial grid cell coordinates 7 

int €rans. index; j= Trens formed scell. index" 


prantt ("Rotaeing by (ei) \n" , 
radineta — 


Pringi inittiale quid: je 


dtheta); 


dtheta * M DEG2RAD; 


grid _display(gridl, SONAR HEIGHT, 0.0, 0.0); 


Prints ("Hib “<cerurn wn 
getchar (); 


grid clear(gridz); 


for (x =O; x <-oridlensize 0] > xt+) 4 
fon ty = Of V <soridlmsize iy yt) 4 
hor (2 =-0;> < gradi.msize 2] 47 -ztr) | 

GrLaZwor vd (Gril, 3, Vy 27 56ux, Gwy owe) 
ax = wx = “rid! cx; 

dy ="Wy =—grid cy; 

© = hypor(ay,.dk); 

theta = atan2(dy, dx); 

Ex = sGridis.ex + © * cos (theta + 2adtheta); 


ry Gigrcileey 4 as 


trans 2andex = 
i Eres: mde x: i= 


world2index(gridl, 
=a 


Sin(theta + radtheta); 


CR EV 7 wc 


grid2.mapm[ trans index] = 
gridlamapm gridZindex (gqmidla-x, yz) 


} 
} 


PrIintL(' Rotated gridin’ ji; 


grid_display(grid2, SONAR_HEIGHT, 0.0, 0.0); 


PEIntel Hie <seturn wn) 
getchar (); 


double grid_match(Map3D stm, Map3D local) 


Match score */ 


/* World coords of match point */ 
Grid cell -coordindtes of March) point. 7 


Index of cell in <stm */ 


/* 
Match two (aligned) evidence grids 
win 4 
{ 
double score = 0.0; [= 
double wx, wy, wzZ; 
ie <p eee ha 
Ine Stmiuindex; i 
int. pl, 2; 


/* Corresponding probabilities */ 
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1387 
1388 
1389 
1390 
139] 
392 
1393 
1394 
1595 
1396 
39 / 
1598 
1599 
1400 
1401 
1402 
1403 
1404 
1405 
1406 
1407 
1408 
1409 
1410 
1411 
1412 
1413 
1414 
1415 
1416 
1417 
1418 
1419 
1420 
142] 
1422 
1423 
1424 
1425 
1426 
1427 
1428 
1429 
1430 
1431 
1432 
1433 
1434 
1435 
1436 
1437 
1438 
1439 
1440 
144] 
1442 
1443 
1444 


ae om /* Match point value (log scale) */ 
int sum = 0; /* Sum of match points values */ 
InitetOtal = U> /~ Total # Of known. points */ 
for (2 


0; x < local.msizef O]; x++) { 


TiOie ry, O; vy < Mocal msizell; yr+) { 
for (2 = O> z< local.msizel 2) > z+): { 
pl = local.mapm[ grid2index(local, x, y, 2)]; 
grid2world(local, x, y, Z, &wx, &wy, &wz); 
stm_index = worldZ2index(stm, wx, wy, wz); 
if s(stmainaec == 1) 
p2 = stm.mapnm{ stm_index] ; 
EOEaLT+T: 
if (((pl < 0) && (p2 < 0)) II 
({pl > 0) “66 {p22 = -0)) |! 
((pl == 0) && (p2 == 0))) { 
Sumas, 
} 
} 
} 
} 
} 
score = (double) sum / (double) total; 
ae score = (double) sum / (double) (local.msizef 0) * local.msizef 1} * 
Local.msizel Zt i 7 
[= Prince (orid match: sum = cd > seore — Si\n";, sum, Score)i;*/ 


return(score); 


} 


double trans_match(Map3D global, Map3D local, double *bx, double *by, 
double *btheta) 


/* 
Transform and match two evidence grids 

ay 

{ 
Map3D local t; /* Translated tocal gra +7 
Map3D local tr; /* Translated/rotated local crid */ 
double score; /* Current match score */ 
double best_score; /* Best match score over all transforms */ 
double dx, dy; /* Current translation distance */ 
double dtheta; /* Current rotation angle */7 
gdouble Dest x7 Dest, /> Best Current Cranslartons 7 
adouble “pest theta; /* Best current rotation */ 
double vx, vy, vtheta; /* Transtormaction vector «7 
ine Sk, Sy% /* Translation step counter */ 
int stheta; / ROtaLION Step COouncery 


Snigeinie (¢locatin, Jocal tex, local evi; 
CUI t eC locdintiy ocalrc  localacy), 


Vxu=o 0.20; 

Wy —10 20; 
vtheta = 0.0; 
do { 


best_score = 0.0; 
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1445 
1446 
1447 
1448 
1449 
1450 
1451 
1452 
1453 
1454 
1455 
1456 
1457 
1458 
1459 
1460 
1461 
1462 
1463 
1464 
1465 
1466 
1467 
1468 
1469 
1470 
147] 
1472 
1473 
1474 
1475 
1476 
1477 
1478 
1479 
1480 
1481 
1482 
1483 
1484 
1485 
1486 
1487 
1488 
1489 
1490 
149] 
1492 
1493 
1494 
1495 
1496 
1497 
1498 
1499 
1500 
1501 
1502 


for (sx = -NUM TRANS; sx <= NUM TRANS; sxt++) { 
for (sy = -NUM TRANS; sy <= NUM TRANS; sy++) { 
form (stheta = -NUM ROT; stheta -—- NUMeROl. sebera..) 4 
Gx = TRANS STEP = (deul ke) sx; 
dy = TRANS STEP ~ (double) sy, 
dthetat= ROT STEP * “double (Sstheta, 
griditranslate (local, lecalvi wx Gx Vyonedy); 
Grigmecotate (local t, locale Veneta tedencea), 
SCOGee—= Guid maccn(¢ obs loca ler a, 
printf ("translation \(4f, ¢£) / Forationm etic escomen. 
Sen 
ax, Gy, dtheta, score); 
LE *(seeme > best Score) | 
Dest yscore — SeOre, 
Desc = os) 
best_y = dy; 
best weicea— Ginera, 
} 
} 
} 
} 
Vit eS We, 
Vy += beseuy; 
vtheta += best theta; 
printf ("BEST translation (%f, %f) / rotation (%f): score = tivne 
BeStie woecey, Dest Enera bese score: 
} 
while((best x != 0.0) || (best y != 0.0) || (best theta != 0. 0)8e 
*DxX = VX; 
“Dy = - Vy > 
PDOEReC ta = VEReca, 


return (Dest. score); 


} 


VOLG Grid copy (Map3D gridl,. Map3D °guzdZ) 
/* 
Copy <dli1d2> -toucorica 


ie 
{ 
double wx, wy, wz; /* World coords */ 
iat 7 ce /* Grid cell coordinates */ 
iniese: /* “Cell Oecctpancy Probability */ 
for {20> 07 x Ggridilimsiza-0); x2) { 
Lor {y= 0; ys gqridi.msize is yv++) 4 
fOr (2 = 02 2 << Gridi.msi ze -2) 7 2) 4 
GrildaZworldigqridi, x,y, 27 Gus, ewy, &wzZ)? 
Gridi-mapm grrazindex(gqridil, x,.y,.42)4. = 


grid2.mapm[ world2index(grid2, wx, wy, 
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WZ). 


} 


void grid fine to coarse(Map3D fine, Map3D coarse) 


{ 


oe 
Lh 
IE 
ie 


} 


double wx, wy, WZ; /* World coords */ 

esas 27 /* Grid cell coordinates */ 

ine p- /* Cell sceupancy probability */ 

int findex; /* Index of cell in fine grid */ 
int cindex; /* Index of cell in coarse grid */ 


Nee yar Gy src 2 f 
grid clear(coarse); 


for {x = 0; x < Line.msizel Ol)s xt4) 7 


for (y = 0; y < f£ine:-msizel 1); yrt) { 
for (2 = 0] 2 < fine, msizel Zi; Ztt) 4 
findex = grid2index(fine, x, y, Z); 


grid2world(fine, x, y, 2, &wx, &wy, &w2); 
cindex = world2index(coarse, wx, wy, w2Z); 


world2grid(coarse, wx, wy, wz, &cx, &cCy, &CZ); 


if (fine.mapm{ findex] < 0) { 
coarse.mapm[ cindex] -= F2C CLEAR WT; 
} 


if (fine.mapm{ findex] > 0) { 
coarse.mapm{ cindex] += F2C OCC WT; 
} 


if ((coarse.mapm[ cindex] == 0) || 
(coarse.mapn{ cindex] < fine.mapm[ findex] )) { 
coarse.mapm{ cindex] = fine.mapm{ findex] ; 


} 


} 
} 


} 


void integrate grid(Map3D global, y* “Globalagrid 7 


aes 
/* 


* 


{ 


MapoD Léealy— /* Local’ grid */ 


double lox, f= Ms0CGal X=Ori Gime fece) = 7 
double loy, /* Local y-origin (feet) */ 
double lotheta) /* Local origin rotation (degrees) 


Imteoqrate <locall> grid Wwithin’<clobal- grid 
/ 


/* Note: Assumes global origin is at (0,0,0) and only handles 
rotations in the xy-plane */ 


deouple wil, Ely a kz /* Local coords (Cartesian) */ 
double lr, ltheta; /*  Geeal coords pdllaz).-/ 
double wx, wy, wZ; J* Wor la co6ras: */ 

double ix, iy, itheta; /* Intermediate coords */ 
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1561 
1562 
1563 
1564 
1565 
1566 
1567 
1568 
1569 
1570 
I571 
hoe 2 
73 
1574 
ley 
ls 
1577 
1578 
eo 
1580 
1581 
1582 
1583 
1584 
1585 
1586 
1387 
1588 
1589 
1590 
ls] 
Tao2 
1593 
1594 
1595 
1596 
1597 
1598 
1599 
1600 
1601 
1602 
1603 
1604 
1605 
1606 
1607 
1608 
1609 
1610 
1611 
1612 
1613 
1614 
1615 
1616 
1617 
1618 


Int Rey, se /* Grid cell coordinates */ 
ier} /*- Cell occupancy probabaliecy *7 
int global index; /* Index -ofreqlobal quid cell’ 7 
int local index; fm Index of local Guid coli: 7 


printf ("integrate Grid: x = tf = y = #£ ; theta = *£\n" > Pox, 7loy- 
lotheta); 


lotheta *= M DEGZRAD; /* Convert to radians */ 


(x = 0; x < global.msizef{ 0) ; x++) { 
for (y = 0; y < global.msize[ 1]; y++t) { 
for (z = 0; z < global.msize[ 2] ; z+t++) { 


/* Convert cell index to global coords */ 
GridZ2world(gqlobal," x,y, 2, 6x, &WY,- SWZ) ; 


/* Convert global coords to local coords */ 


1x = wx - lox; 
iy = wy - loy; 
itheta = atan2(iy, ix); 


iy = hyporcEx,. 1y):; 
ltheta = itheta - lotheta; 


ix = In cos (leheta); 
ly = lr * sin(ltheta); 
1z = wZ; /* Assume z-coord is unchanged * / 


/* print ("global (sf, ° sf) ===. local. (tt, Filme 7 wee ee 


/® Update. gioball ceri */ 


im MIN) oe f= MEX) 6 
(ly >= Y MIN) &@& (ly <= Y MAX) && 
lia >= 2oMIN je ee (2 = 7 ae) 
glebal index = oridZindex(global, x, y, 2); 
local index = world2index(local, 1x, ly, 12); 
global.mapnf{ global index] += local.mapn[ local index] ; 


if (global.mapn[ global index] > POS) { 
global.mapn{ global index] = POS; 

} 

else if (global.mapm{ global index] < NEG) { 
global.mapm{ global index] = NEG; 


vold integrate global grid(Map3D global, /* Initial) global grid. | 


Map3D global _ new, /* New global grid */ 
double nox, /* Local x-origin (feet) */ 
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G19 
1620 
1621 
1622 
1623 
1624 
1625 
1626 
1627 
1628 
1629 
1630 
1631 
1632 
1633 
1634 
1635 
1636 
1637 
1638 
639 
1640 
1641 
1642 
1643 
1644 
1645 
1646 
1647 
1648 
1649 
1650 
1651 
1652 
1653 
1654 
1655 
1656 
1657 
1658 
1659 
1660 
1661 
1662 
1663 
1664 
1665 
1666 
1667 
1668 
1669 
1670 
1671 
1672 
1673 
1674 
1675 
1676 


double noy, /* Local y-origin (feet) */ 
double notheta) /* Local origin rotation (degrees) 
re 
/* 
Enteqrate <local> grad within <global> grid 
af 


/* Note: Assumes global origin is at (0,0,0) and only handles 
rotations in the xy-plane */ 


double nx, ny, nz2; /* New global coords (Cartesian) */ 
double nr, ntheta; /* New Global coords (polar) */ 
double wx, wy, WZ; /* World coords */ 

double ix, ly, itheta; /* Intermediate coords */ 

apr ee yc /* Grid cell coordinates */ 

iio j= Celt occupancy prebability «7 

int global index; /* Index of global grid cell */ 

int new_index; /* Index of new global grid cell */ 


printf("integrate grid: x = $f : y = $f : theta = %f\n", nox, noy, 
notheta); 


notheta *= M DEG2RAD; /* Convert to radians */ 
for Wx = 0S “ = Global.-msizel 0].7 xt) 4 
for (y = 0; -y <-global-msizel li 7 yt) { 
fOr (2 = 07 2 <2Gglobal-msize 27 zt) 


/* Convert cell index to global coords */ 
grid2world(global, x, y, Z, &wx, &wy, &WZ); 
/* Convert global coords to new global coords */ 


ix = wx - nox; 
iy = wy = noy; 
itheta = atan2(iy, 1x); 


Dre Myporlix, 1y)) 
ntheta = itheta - notheta; 


Nx = one ~*~ cos (ntheta) > 
ny = nr * sin(ntheta); 
NZ = w2Z; /* Assume z-coord is unchanged */ 


/* Pprintit (clobal (si si) +=-> new global (Sf,-.52) in" | wx, wy, 
By iy 7 


/* Update global cell */ 


if ((nx >= GLOBAL X MIN) && (nx <= GLOBAL X MAX) && 
(ny >= GLOBAL Y MIN) && (ny <= GLOBAL Y MAX) && 
(nz >= GLOBAL Z MIN) && (nz <= GLOBAL Z MAX)) { 
global index = grid2index(global, x, y, 2); 
new_index = world2index(global new, nx, ny, nz); 
global.mapn{ global index] += global_new.mapnf{[ new index] ; 


if talebal-~mapn global index) =sbOs) 4 
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global.mapnm[ global index] = POS; 

} 

else if (global.mapm[ global index] < NEG) { 
global.mapm[ global index] = NEG; 


void save _grid(Map3D grid) /* Evidence grid */ 
/* 
Save evidence grid to file 
nu 
{ 
char filename[ 80] ; /* Save file name */ 


printl( Enter save tile name ——— ")- 
scanf(" %@s", filename) ; 


printf("Saving grid to <%s>.\n", filename); 


WriteMap3D(grid, "Evidence Grid", "", filename) ; 
} 
void save_grid_file(Map3D grid, /* Evidence grid */ 
char *filename, /* Save file name */ 
char *comment) /* File header comment */ 


/* 

Save evidence grid to specified file with header comment 
ry 
{ 


Printi (Saving grid to <ts>. \n",. £ivename); 


WriteMap3D(grid, comment, "", filename) ; 

} 

void load grid(Map3D * grid) fo SE vrdence- Grid. *./ 

/* 

Load evidence grid from file 

ner 

{ 
char filename 80] ; /* Load file name */ 
char titlel 80], footer[ 80] ; /* Discarded */ 
printf("Enter load file name ==> "); 
scanf(" %s", filename); 


printf ("Loading grid from <%s>.\n", filename) ; 
if (ReadMap3D(filename, grid, title, footer) == 0) { 
printf("load grid: Unable to load grid from <%s>.\n", filename); 
} 
} 


int load grid file (MapsD * grid; /* Evidence grid */ 
char *filename) /* Load file name */ 
/* 


Load evidence grid from specified file 
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37 
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ey 7 1 
ne 72 
7 73 
1774 
1775 
1776 
777 
1778 
n7 79 
1780 
1781 
1782 
1783 
1784 
1785 
1786 
1787 
1788 
1789 
1790 
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792 


Returns 1 if successful, O otherwise 
af 
{ 
char title{ 80], footer[ 80]; /* Discarded */ 


printf("Loading grid from <%s>.\n", falename); 

if (ReadMap3D(filename, grid, title, footer) == 0) { 
Printt(” lead grid: Unable to load grid from <ss>.\n",- fileneme); 
meturn (0); 

} 

return (1)? 


} 


Mie Load grid file com(Map3Pp = ¢r1d, {/* Evidence grid */ 
char *filename, /* Load file name */ 
char *comment) /* Comment string */ 

/* 


Load evidence grid from specified file along with header comment 


Returns 1 if successful, O otherwise 
cy 
{ . 
char footer[ 80]; /* Discarded */ 


printf("Loading grid from <%s>.\n", filename) ; 

if (ReadMap3D(filename, grid, comment, footer) == 0) { 
printer (lead guid; Unable to: load guid) fzom <ss>. vn; , Liidename), 
return (0); 

} 

return(l); 


} 


wenadwgrid Count voce (Map3sD grid, Int ~oecc, ine ~unocc) 
/* 

Count number of occupied and unoccupied cells in grid 
e/ 
{ 


MES; Vp 2: /* Grid cell coordinates */ 
int xsize, ysize, zs1ze; /* Grid dimensions (#cells) */ 
Anes .p; /* Céll occupancy probability */ 
xsize grid.msizef[ 0] ; 


Vsizce Grid.msizel 1) ; 
zsize grid.msize[ 2]; 
* OCG. — 0; 
Umoce — ©; 
for (x.= 0; x < xsize;”"x++) { 
FOr ViVe=H=E0) Ve ysize-eya) { 

for (z = 0; z2 < zsize; z++) { 

Dp =“Grid.mapml z>* aysizey* xsize + y *)xsizeurt ix); 

ae es vO) Fl 

(2 OCe a: 


else if (p < 0) { 
C]Uunoecec) ++. 


} 
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APPENDIX E. FRONTIER-BASED EXPLORATION CODE — ROBOT.H 


This appendix contains the header file for the routine that controls many of the robot’s 
basic movement behaviors. 


/* 
wObet .N 


Header file for robot class for Nomad 200 Simulator. 
Original code by Brian Yamauchi 


Modifications for SCOUT THESIS 
By Patrick A. Hillmeyer 


may 
#ifndef ROBOT H 
#define ROBOT H 


#include "Nclient.h" 
#include "vector.h" 
#include "misc.h" 

fomcilude.  Ggrid++.. i" 


// BEGIN SCOUT THESIS CHANGE 


// These are the conversion macros from Nomadic that accept the steering 
and 

// translation values as used for the Nomad 150 and 200 and calculate 
the 

j/?/ Oitferential—-drive axis values Eor the Nomad Scout. 


#define ROTATION CONSTANT 0.118597 /* inches/degree (known to 100 
pom). * / 

#define RIGHT(trans, steer) (trans + 

(int) ((float) steer* ROTATION CONSTANT) ) 

#define LEFT(trans, steer) (trans - 


(int) ((£loat) steer* ROTATION CONSTANT) } 


#define scout _vm(trans, steer) vm(RIGHT (trans, steer), LEFT(trans, 
steer), Q) 
#define scout_pr(trans, steer) pr(RIGHT (trans, steer), LEFT(trans, 
Steer), 0) 


// END SCOUT THESIS CHANGE 


const int NUM SONAR = 16; // Number of sensors of each type 
const int NUM_IR = 16; // Actually O for SCOUT but leave for now 
const int NUM _ RANGE = 16; 


// BEGIN SCOUT THESIS CHANGE 


17] 


110 


const int NUM TOUCH = 6; // Scout only has 6 bumper swithes 
// END SCOUT THESIS CHANGE 
const int NUM ARC = 16; // Number of sensor arcs 
const int ARC SIZE = 3; // Number of sensors in each arc 
const ant. ARC] tke — | // Interval between first sensor 

// in each successive arc 
const int ARC OFPRSET = —1, // Value of first sensor of first arc (mod 
16) 
const int SONAR_ADDR = 17; // State index for first sonar sensor 
const int IR_ADDR = 1; // State index for first IR sensor 
const int TOUCH ADDR = 33; // State index for touch sensors 


const int LASER MODE ADDR = 42; // State index for laser mode 
const int MAX_SONAR = 255; // Maximum sonar reading 


// BEGIN SCOUT THESIS CHANGE 
const ant MAX IR =_0; // Maximum IR reading —- no IR on Seonup 
// END SCOUT THESIS CHANGE 


const int MAX RANGE = 255; // Maximum range reading 


const int SENSOR SEP = 225; // Separation between adjacent sensors 
// in degrees/10 

// BEGIN SCOUT THESIS CHANGE 

// this mext setting for BUMPER SEP can be left as is fo~ mow evem 

// though it is wrong for the Scout because the procedures that use 

// it in agent.cc for recoiling from a bumper contact are not 

implemented yet 

// New NOTE - changed to 600 to fake out some code in robot.cc 

// Needs a better fix though 

const int BUMPER _SEP = 600; // Separation between adjacent bumpers 

// in degrees/10 


const int MAX SPEED = 200; // Maximum velocities 
const int MAX TURN RATE = 300; // From Nomadic .setup file for Scouts 


const int MAX ACCEL = 300; // Maximum accelerations 
const int MAX TURN ACCEL = 500; 


const int DEFAULT SPEED = 200; // Default velocities 
const int DEFAULT TURN RATE = 300; // From Nomadic .setup file for 
Scouts 


// END SCOUT THESIS CHANGE 


const int DEFAULT ACCEL = 200; /7 Detaule acceleraurons 
const int DEFAULT_TURN_ACCEL = 500; 


const int MOVE_TO SPEED = 10; // Speed for moving to (x,y) 

const int FACE TURN RATE = 200; // Turning rate for facing 
const int MAX CONT TURN = 225; // Maximum turn without 
stopping 

const int FACE CONT WAIT = 10; // How long to wait for turn 


EOL iis 
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129 
130 
131 
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134 
135 
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138 
139 
140 
141 
142 
143 
144 
145 
146 
147 
148 
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155 
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const int ROBOT MIN SPEED = -200; 
const int ROBOT MAX SPEED = 200; 
const int ROBOT MIN TURN = -100; 
const int ROBOT MAX TURN = 100; 


// BEGIN SCOUT THESIS CHANGE 


// Velocity limits (command) 


// NOTE - changing no encoder parameters for now but might need to 
v7 tweak them for the Scouts 


// Dead reckoning parameters 


const int ENCODER COLOR = 19; 

// Minimum/maximum encoder rotation 
//const double ENCODER ROTATE MIN = 
//const double ENCODER_ROTATE MAX 


const double ENCODER ROTATE MIN = 0. 
gee 


const double ENCODER ROTATE MAX = 


77 Encoder rotation bias 
Gonst double ENCODER ROTATE BIAS = 
//const double ENCODER ROTATE BIAS 


0.02 
= 0. 


// Color of encoder graphic 


increment 
leo; 

10> 

oF 


O01; 


// Minimum/maximum encoder translation increment 


//const double ENCODER_TRANS MIN 
//const double ENCODER_TRANS MAX = 
eonst double ENCODER TRANS MIN = 0. 
const double ENCODER TRANS MAX = 1 


il 


v7 Pacoder translation bias 


const double ENCODER_TRANS BIAS = 0. 


//const double ENCODER TRANS BIAS 
// Cartesian move parameters 

GOnstG lint MOVE XY MAX DIST = 1200; 
const int MOVE XY MAX ERROR = 1; 
inches) 

// END SCOUT THESIS CHANGE 

// Building Axis Direction 

const int AXIS = 2960; 


j// AEC Gi rections 


enum { 
BACK, BBR, BACK RT, BRR, RT, 


// Timeout for movement commands 
const unsigned char MOVE TIMEOUT = 
// Robot class definition 


class robot { 


varies 
Oe 
oe 
lg 


OF 
0.001; 


(EGntEas Inenes) 
(tenths 


// Maximum move 
// Maximum move error 


PWD ae hin SEWDOLE ELL, be, BE, BACK LE, BBL, 


FRR, FWD RT, FFR }; 


LOO: 


169 
170 
al 
172 
173 
174 
lee 
176 
177 
178 
179 
180 
18] 
182 


184 
185 
186 
187 
188 
189 
190 
19] 
2 
iS 
194 
195 
196 
1O7 
198 
199 
200 
201 
202 
203 
204 
205 
206 
207 
208 
209 
210 
211 
212 
215 
214 
215 
216 
ae 
218 
210 
220 
22 | 
Z22 
223 
224 
27> 
226 


public: 
imtsiad; // Robot ID number 
int. x7. tb ncka, Eur er, // Robot encoder position 
int actual_x, actual_y, actual theta; // ROBOE “acttial pesztt1on 
double €ncix, ene ly, “ene eheta; // Accumulators for encoder position 
int bumper offset; // Offset betwen base and bumpers 
double trans ctr; // Total translation since 
localization 
double rot ctr; // Total rotation since localization 
int Originox, Orrg ini, // ROOn ori6in 
int bumpers; // Bumper bit vector 
vector sonar; // Sensor values 


vector ix; 
vector range; 
VECLOr Couch; 


vector abs_ sonar; // Absolute sensor values 
vector abs ir; 

vector abs. range; 

VECEOr dbs touch, 


vector arc; // Sensor arcs 


/* 
vector sonar(NUM_ SONAR) ; // Sensor values 
VECEOr Jr NUM SER) 
vector range (NUMARANGE),; 
Vector Eouch (NUM TOUCH); 


vector abs _ sonar (NUM_SONAR) ; // Absolute sensor values 
Vector, ans of NUM UR); 

vector abs range(NUM RANGE); 

vector abs couch (NUM TOUCH); 


vector arc(NUM ARC); // Sensor arcs 
x / _ 

robot (void) ; // Gensecuccor 

void update (void); // Sensor update 

vold set_default velocity(void); // Set default trans/base/turiee 
speed 

void Maint err (yvold),; // Maintain encoder error at new posieieg 


// Relative move of <speed>/10 inches forward while turning both 
EUrreLu 
// and base by <angle>/10 degrees (+ = ccw, - = cw) 


void move(int speed, int angle); 


// Relative move of <speed>/10 inches forward 
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ZZ] 
228 
29 
230 
Z31 
232 
233 
234 
Zz) 
236 
23 / 
238 
259 
240 
241 
242 
243 
244 
245 
246 
247 
248 
249 
250 
251 
ZO 
253 
254 
Zo) 
256 
Z>/ 
258 
Zoo 
260 
261 
262 
263 
264 
265 
266 
267 
268 
269 
270 
271 
2/2 
23 
274 
Z15 
276 
Zi] 
278 
279 
280 
281 
282 
283 
284 


void fwd(int speed); 

// Turn base by <angle>/10 degrees (+ = ccw, - = cw) 
void turn(int angle); 

// Set origin to current position 

Word Set .onigin Nere(vord); 

f/f set Origin te (Gx, ©. y) 

Vora See Origin, loctine oO x, ant Oo Vy); 

// Convert angle to sensor index 

int theta2sensor(double theta); 

// Convert sensor index to angle 

double sensor2theta(int sensor); 

// Return 1 if all range sensors in an arc <width> x 2 sensors wide 
// centered around sensor <ctr> return greater or equal to <dist>, 
// 0 otherwise. 

mt check clear(int CUr, 2nt width,-int dist), 

// Wrap index to [ 0..NUM_ SENSORS] 

int sensor wrap(int index) ; 

// Turn off sensors 

void shutdown (void); 

7 MMOVeECECHOL tC <x; yo (world coords, tenths of inch) 
EGE AMoOvereO. XY Int Cx, Int cy) 

// Turn robot to face <angle> accurately 

“wold fFacevang le (intwang le); 

// Turn robot to face <angle> quickly 

Vold pr acevanelertase(inerang le), 

// Turn robot to face <angle> quickly, without stopping 
VordsFacevanglescone (ime rang te); 

// Align turret with base 

VOlG) Turret aligniveld); 


// Relative Cartesian move to <x, y> (world coords, tenths of inches) 
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296 
29] 
298 
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300 
301 
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305 
306 
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308 
309 
310 
311 
oe 
a3 
314 
a 
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Sy 
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319 
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void move rel(int x, int y); 
// Sensor functions 


void sonar_on(void) ; 

void sonar_single(int index); // Index of sensor to fire 
void sonar_off (void); 

void ir _on(void) ; 

void ir single(int index) ; // Index of sensor to fire 

Void, 47 Ori (Vo1d):; 

void laser _on(void); 

void laser off (void); 


// Wait for the robot to start moving (any motor) 


void wait start (void); 


private: 


Ie 


/7 Mnitielization functions 
void initialize sensors (void); 
// Update functions 

void update dr(void); 

void update range arcs(void); 


void update arc(int é&av, int first, int last); 


// Cleanup functions 
void deactivate sonar (void); 


#endif 
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APPENDIX F. FRONTIER-BASED EXPLORATION CODE —- ROBOT.CC 


This appendix contains the source code for the routine that controls many of the robot’s 
basic movement behaviors. 


cs 
rODOE cc 


Robot class for Nomad 200 Simulator. 
Original code by Brian Yamauchi 


Modifications for SCOUT THESIS 
By Patrick A. Hillmeyer 


na 


#include <iostream.h> 
tincluce <math.h> 


timneclude "“robot.h" 
#include "drand.h" 
Pinecilude “irand.h" 


// Dead reckoning mode (actual, independent, or error) 
#define DR_ACTUAL 
// Touch vector mask 
Conse int touch mash] 20) = { 1, 27 4, 8, 16, 
Se, Oo 228, 256, ole; 


1024, 2048, 4096, 8192, 16384, 
32/60, 62556, Pai07 2, 2621447524238 4; 


// Forward contact mask 
Const int FWD CONTACT = 1015839; 


robot: : robot (void) :sonar(NUM SONAR), ir(NUM_IR), range(NUM_ RANGE), 
touch(NUM TOUCH), abs sonar(NUM SONAR), 

abs ir(NUM GIR), 
abs _range(NUM RANGE), abs_touch(NUM TOUCH), 

arc (NUM ARC) 

{ 


INEGEX, Fy, riheta, J /@RCDCe Home Besition. (710s neh 17 LO 
degree) 


J /PEOCNNSCCEELONSeCE Ver -anagractivate 2llvsencors 


cout << "Enter Nserver host name ==> "; 
Cine SERVER MACHINE NAME; 


Gouct<a< “Enter Nserver robot 1) —=> 
Cine >w ice 


GOnnect. robot (1d); 
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53 initialize sensors(); 


a set default velocity(); 
SD 
56 f/f Inver Lalize origin 
57 
58 origin x = 0; 
a9 origin y = 0; 
60 
61 // initialize. translation/rotation counters 
62 
63 trans cer = 0-0, 
64 COE CULL =m .e, 
65 
66 Ji Gere thescobper 
67 
68 fF. SEE Ys 
69 
i J//*Set cobot initial position 
l 
a2 je ello me.” \- 
73 
74 Cout << “Pnter robot x y theta (no commas) ——— —- 
75 Cin o> iex = ry o> pinera- 
76 
ai gsi): 
te bumper offset = State{ 36] - rtheta; 
place obor(rx,. ry, EEheta, Ethera); 
82 // Initialize actual position 
83 
84 gs(); 
85 actual _ x = State{ 34]; 
86 actual _y = State[ 35); 
87 actual sthetas— state) (30): // DR heading 


88 // actual theta = 3600 - wrap(State[ 43] - AXIS, 0, 3600); // Compass 
89 heading 


90 

9] // Initialize encoder accumulators 
92 

93 enc x = (double) actual_x; 

94 enc_y = (double) actual _y; 

95 enc theta = (double) actual theta; 
96 7 

97 // Initialize estimated position 

98 

99 x = round(enc x); 

100 y = round(enc y); 

101 theta = round(enc_ theta) ; 

102 

ia // Display robot estimated position 
] 

Te // draw_robot(x, y, theta, theta, ENCODER COLOR); 
107 // Updater robot state 

108 

109 update (); 

FLO: 4 
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125 
126 
ie / 
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133 
134 
> 
136 
137 
138 
9 
140 
14] 
142 
143 
144 
145 
146 
147 
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149 
150 
151 
2 
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> 
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IS 
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9 
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165 
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void robot: :maint_err(void) 


{ 


} 


// Maintain encoder error at new position 
int ex, ey, etheta; 
ex = x - actual x; 


ey = y - actual y; 
etheta = theta - actual theta; 


gs(); 
place robot (Statel 34) 5 state So), Starel 3c), States) 7; 


actual x = State 34] ; 
actual y = State 35] ; 
actual theta = State[ 36] ; 


x = actual x + ex; 
y = actual _y + ey; 
thete = actual theta + etheta;, 


VOie —ObOts> Set -cCetaule yelociey() 


{ 


} 


sp (DEFAULT SPEED, DEFAULT TURN RATE, 0); jf TEMe Fix tor scour 
ac(DEFAULT ACCEL, DEFAULT_TURN_ ACCEL, 0); // TEMP FIX for SCOUT 


void robot: :update (void) 


{ 


// Update values for position <x, y, theta>, sonar <sonar>, infrared 
// sensors <ir>. Also update range arcs. 


int range offset; // Rotation offset for range sensors 
ime touch offset; // Rotation offset for touch sensors 
aE eh cls 

gs(); 

update dr(); 

range offset = (int) ((double) theta / (double) SENSOR_SEP + 0.5); 


// NOTE - need to fix this BUMPER _SEP dependency later for SCOUT 


touch offset = wrap((int) ((double) (theta + bumper offset) 
/ (double) BUMPER SEP + 0.5), 
NUM_TOUCH) ; 
/ f° SCOUET <<)" Of Esets— "<< range oftsers<<. "' "<< endl; 


for (1 = 0; 1 < NUM_SONAR; i++) { 


sonar[ i] = State[i + SONAR _ADDR] ; 
abs_sonar[ i] = State[wrap(i - range offset, NUM_SONAR) + 
SONAR ADDR] ; 
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168 
169 
170 
171 
172 
eo 
174 
175 
176 
ley 
178 
19 
180 
18] 
182 


184 
185 
186 
187 
188 
189 
190 
191 
192 
193 
194 
b> 
196 
ioe 
198 
199 
200 
201 
202 


204 
205 
206 
207 
208 
209 
210 
211 
Zee 


214 
Ze 
216 
Zid, 
218 
Z\9 
220 
22 
222 
225 
224 


ie Gout << “2 =" << 1°-<< "+: range Offset 1 = " <— wrap 
range offset, NUM_SONAR) << 
// me SOnarn << 16%) =" << conan i) ea eee ence 


} 


// BEGIN SCOUT THESIS CHANGE 
// Comment out IR code and only depend on sonars 


// SCOUT THESIS CHANGE - correct error where sensor updates below were 
1@tE OuteoL Woop 
fOr -( 2 = 07 NUM SONAR; la) -{ 


abs range[i] = abs_sonar [i]; 

range{[ i] = sonar[ i]; 
// cout << "Just set by sonar[ i] value : range[" << i << ") =" << 
range[ i] 
1 << endl; // TEMP FIX 
// cout << "Just set by abs _sonar[ i] value : abs range[” << i << “je 
ay << abs _range{i] << endl; // TEMP FIX 


} // end for loop 


ji £00 429 =— 070 2 < NUM TR? tte) 


Ty ir[ i] = State{[ i + IR ADDR] ; 

he abs irfi] = State[ wrap(i - range_offset, NUM_IR) + IR_ADDR]; 
// eout << "i=" << i << " + range offsec a = "<< syrocoee 

range Of fseu,) NUMPIR << 
V/. ee er sce ee ree aaa me re eee 

aa 

Jf £0042 = "0 32 < NUMZRANGE? i++) { 

/7 if (abs ir{ i] < abs_sonar[i]) { 

Tf abs _range[ i] = abs _irf i); 

ii } 

Va. else { 

iy abs _range[{ i] = abs_sonar[ i]; 

fy } 

iy. 

77 if Cir 21) = sonar -2)) 4 

v7 range{ i] = ir{ i}; 

ff] } 

if else { 

// range[ i] = sonar{ i]; 

ia } 

if 3 


// END SCOUT THESIS CHANGE 


for (i = 0; 1 < NUM_RANGE; itt) { 
if (range{ i] > MAX RANGE) { 
range[ i] = MAX_RANGE; 


// cout << "Compared against MAX RANGE : range[" << i << "] =" << 
range[ 1] 
1a) << endl; // TEMP FIX 


} 
if (abs_range[{ i] > MAX _RANGE) { 
abs range{[ i] = MAX RANGE; 
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226 
22] 
228 
229 
230 
D3 | 
m2 
233 
234 
2) 
236 
23] 
238 
259 
240 
241 
242 
243 
244 
245 
246 
247 
248 
249 
250 
251 
ap 
253 
254 
2) 
256 
ED / 
258 
259 
260 
261 
262 
263 
264 
265 
266 
267 
268 
269 
270 
271 
Z)2 
Z/3 
274 
2/5 
276 
ZiT 
278 
AR 
280 
28] 
282 


// cout << "Compared against MAX RANGE : abs range{" << i << "] =" << 
abs _rangef i] 
Le << endl; // TEMP FIX 
} 
} // end for loop 


Updates range ares (77 


bumpers = State{ TOUCH ADDR] ; 


// if (bumpers != Q) { 
Mee cout << "Bumper state =" << bumpers << "" << endl; 
ae 


// NOTE - touch_offset depends on BUMPER SET - needs fix for SCOUT 


fori — O07) a NUM. TOUGH itt) 4 
if (bumpers & 
touch_mask[l wrap(i + touch_offset, NUM_TOUCH)] ) { 
touch! 1) = 1 


cout << "Contact on bumper " << i << " (abs index =" << 
Weap (i + Couch: Ofrser, NUM TOUCH) <<) << endl? 

} : 

else { 


touch{ i] = 0; 
} 
} 


7 seo << "Sonar " << sonar << endl; // TEMP FIX 
ly “COoue <<." Ik << een meoncue: 


// cout << "Range =" << range << endl; // TEMP FIX 
Ve weoutk << “Ares =" << are <a endl, // TEMP FIX 
ie @e00t <<." fouch = ' << ‘tomen << -enal: 


} 


Mole robot: update sdr (vor) 


{ 
// Update dead reckoning 


double dx, dy, dtheta; // Motion since last update 

int dtheta mag; // Magnitude of rotation 

int dtheta_sgn; // Direction of rotation (+ ccw, - cw) 

double vec_r, vec_theta; // Motion vector 

double inc; // Motion increment 

double ctheta, stheta; // Components along x-axis and y- 
axis 

double trans_step; // Length of current translation 
SEED 

iP: 

dx (double) (Starel 34) te ecrucwt sic 


ay = (Gouble es State. 3 o))--sactiain 7); 
dithetas= angle .sqn-ditt((deuble) State (36) 7 10.0, 
(Gombe) actuateeneta sat 0 0) i> SO au), 


State[ 34] ; 
State[ 35] ; 


actual £x 
actual _y 
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283 
284 
285 
286 
287 
288 
289 
290 
Zo 
202 
293 
294 
275 
296 
227 
298 
299 
300 
301 
302 
503 
304 
305 
306 
307 
308 
309 
310 
311 
eZ 
313 
314 
315 
316 
317 
318 
a9 
320 
S21 
oo 
323 
324 
325 
326 
B27 
328 
S29 
330 
331 
poe 
353 
334 
335 
336 
337 
338 
339 
340 


actual theta = State 36] ; // DR heading 


// actual theta = 3600 - wrap(State[ 43] - AXIS, 0, 3600); 


heading 
#ifdef DR ACTUAL 


// Dead reckoning always returns actual position 


x actual x; 

y = actual y; 

theta = actual theta; 
// SCOUT THESIS CHANGE - comment out original turret line 
// set turret to be same as SCOUT heading angle 
// turret = Statef{ 37); 

turret = theta; 


#endif // DR_ACTUAL 


#ifdef DR_INDEP 


// Compass 


// Dead reckoning 1S updated by actual displacements, but may be set 


// independently 

draw robot(x, y, theta, theta, ENCODER COLOR) ; 

S-4t= (int) as: 

yo += (Ant) ay? 

theta += (int) dtheta; 

draw robot(x, y, theta, theta, ENCODER COLOR); 
#endif // DR_INDEP 
#ifdef DR_ERROR 

// Dead reckoning accumulates error over time 

draw _robot(x, y, theta, theta, ENCODER COLOR); 

rot Cir += fabs(athera); 

dtheta mag = (int) fabs(dtheta); 

dtheta_sgn = sgn(dtheta); 

for Ki o> U7 a) < “dGhetea mag; sits) 

ene theta +— (double) dtheta sgn * 
rdrand (ENCODER ROTATE MIN, ENCODER ROTATE MAX) + 

ENCODER ROTATE BIAS; 

} 

Gene theta = angle weap iene eneta 7 10.0) * 1020; 

theta = round (enc Enea); 


vec r = hypot(dx, dy); 


Le VeG ero 0.0) ad 
Llanes Clr += vec 1; 


vec theta = (double) theta / 10.0; 
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34] if (angle diff(vec_ theta, atan2(dy, dx) * RAD2ZDEG) > 90.0) { 


342 vec theta = angle wrap(vec theta + 180.0); 
343 
344 
345 ctheta = cos(vec theta * DEG2RAD); 
346 stheta = sin(vec theta * DEG2RAD); 
347 
348 for (i = 0; i < (int) vec_r; i++) { 
349 trans step = rdrand(ENCODER_TRANS MIN, ENCODER_TRANS MAX) + 
350 ENCODER TRANS BIAS; 
351 trans step = 1.0; 
BD 2 enc x += ctheta * trans step; 
ED. ene y +— Stneta, = Crans step, 
354 } 
BD) 
356 enc x += Ctheta” (vec F— (ane) veer); 
BD / enc y += stheta * (vec _r - (int) vec_r); 
358 
359 Ke= TGeund(enc )4 
360 y = round(enc_y); 
361 } 
362 
a draw_robot(x, y, theta, theta, ENCODER_COLOR) ; 
3 
365 vom Gout. << “hetual: ("<< SCEUG IDR 9, SS veehlat a 
366 actual_theta - 
367 << "Sr = Encoder: (" << "ene -x << ") " << enc.y <<. 9) -<) << enc thera 
368 = << 7 — a 
369 a= rome) aa -CUCs xX. COON Le )ciGi lala a <)uee 
370 enc y - (double) actual_y <<") <" << 7 
37] round(angle sgn diff(enc theta / 10.0, 
5/2 (double) actual theta / 10.0) 
B73 Aon 0) nae! amend: — 
374 
7) eouts-< “Totals translation = " << trans ctr << “ = rotation =a 
376 BoEleee << endl +7 7 
377 
378  #endif // DR_ERROR 
379 
380 Feturn; 
381} 
382 
oa void robot::update range arcs (void) 
{ 
385 // Update range arcs. The value of the arc is equal to the minimum 
Boe // range reading of a sensor that is included in that arc. 
388 ine Migs Sistas, 
389 
390 for (i = 0; i < NUM ARC; i++) { 
391 first = wrap(i * ARC STEP + ARC OFFSET, NUM RANGE) ; 
oa last = wrap(first + ARC_SIZE - 1, NUM RANGE); 
394 anciel) “= "eange mim rinse last) y 
395 } 
396} 
597 


398 =void robot: :sonar_on(void) 
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399 
400 
401 
402 
403 
404 
405 
406 
407 
408 
409 
410 
411 
412 
413 
414 
415 
416 
417 
418 
419 
420 
421 
422 
423 
424 
425 
426 
427 
428 
429 
430 
431 
432 
433 
434 
435 
436 
437 
438 
439 
440 
44] 
442 
443 
444 
445 
446 
447 
448 
449 
450 
45] 
452 
453 
454 
455 
456 


{ 


// Activate all sonar sensors 
int sn_order{ 16] ; /7 SOnae firing order 


/* set firing rate and sequence of all sonar */ 


snveorder| 0] =) 0; sneorder( 1 = 10, (sn sorderiZ)) = 6. 

sn_order{ 3] = 14; sn_order[ 4] = 2; sn_order[ 5] = 12; 

sn order 6] = - 4, sma order, 7) = 9; snvorderi3|) — si, 

sn order 2) .= [37 sn order, 10) — 5; sm order fl], 25, 

SH Orderiel 2) (= 7, ssmnorcen-13) = li snsorden 4) aa. 

SHlorcernh 5), = <s) 

cont Sn tanG SnuCracr); // TEMP FIX SET LONGER SONAR FIRING TIME 


} 


void robot::sonar_single(int index) // Index of sensor to fire 


{ 


// Activate one sonar sensor 


int sn_order{ 16] ; // Sonar firing order 
sn_order[ 0) = index; 
sn order 1 = 255, 


Cont Sh o(/2) siorder)); 


} 
VOL, PODOL. : Sonar Jorn (youd) 
{ 
// Deactivate all sonar sensors 


int sn_order{ 16] ; /7-SOfiar Firing order 


sn_order{ 0] = 255; 
Coni-cn (1, ssiwosder), 


// BEGIN SCOUT THESIS CHANGE 
// let the IRs and laser be configured - just comment out the activation 
// in the following procedures 


VO1d woboe 7211 .6n{ void) 
{ 
// Activate all IR sensors 


Dnt ir Order ol; // IR firing order 


/* set firing rate and sequence of all IR */ 


iz order 0] = Op tiz order I) — 10, ar orden 2 — —G; 
ig order 3) = 14; "ir order 4) = 2; in vorder >) = 12, 
ix ordexm.6] = 4; irverder[ 7] — 97 ariordex é] — ad, 
iy ordexz 3] = 13) if “orderiiO]” = Ya, in order il —sis. 
ix order, 12) = 7? ix order, 13) = diy ar vorden 14] s=.3, 
ir order{ 15) = 8; 

// Cone tei 2,i tr order). 


} 
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457 
458 
459 
460 
461 
462 
463 
464 
465 
466 
467 
468 
469 
470 
471 
472 
473 
474 
475 
476 
477 
478 
479 
480 
481 
482 
483 
484 
485 
486 
487 
488 
489 


491 
492 
493 
494 


496 
497 
498 
499 
500 
501 
502 
503 
504 
505 
506 
507 
508 
509 
510 
al 
p12 
D3 
514 


vould ToDet.-im Single (ant index) // Index of sensor to fire 


{ 


// Activate one IR sensor 


Int 26 order 16]7 {/7 ER farang order 
Trsorden 0) = 2andex, 
treerder i] = 254; 


L/ eOn bh tr (2,1 Orden); 


} 


ot -OOOt.. 1h Ord ivord) 


{ 


// Deactivate all IR sensors 
int ir order{ 16] ; jf ER £irinGg order 


bevorder|O] — 235; 
Mie onG 10(2, BE Order), 


} 


void robot: :laser_on(void) 


{ 


// Activate laser 


// conf 1s(LASER MODE ON, THRESHHOLD, WIDTH, NUMDATA, AVG); 
} 


void robot::laser_ off (void) 


{ 


J / DeActivate Laser 


// conf _1s(LASER_MODE OFF, THRESHHOLD, WIDTH, NUMDATA, AVG); 
} 


WOU erObOELs iit ialrze Sensors (void) 


{ 


/* 
Activate all robot sensors 
ey 
ey Skaticyine (com 6l) t=O, % 107.46, 01422 as OD ee ae 
bee Sect Ole 


aE SEACTC PINES EsOr i M6) |) 2557-17527 3) Bee er eee ee 
to lay Oe 


Stabe sine somone! 6)! sO 72 NO. Gulia 2, vl ee ee oe 
ME te Sh Opie 

Stab Cries Sm sot 16) 295) dg 8 ee eS, Ore ec, 
eS ea mel oo 

Ics 

( “iia SSemsors ()); “<7 


fi Gonieds (Odom li; 
conf osn(10, sn om); "7 /TEMP@hix - sets longer slower firing tame 
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a 
516 
or? 
518 


522 
523 


534 
53) 
536 
D37 
538 
539 
540 
541 
542 
543 
544 
545 
546 
547 
548 
549 
550 
a5 
52 
oe) 
554 
Soo 
556 
oo 
558 
559 
560 
561 
562 
563 
564 
565 
566 
567 
568 
569 
570 
571 
a7 2 


Smask{f 42] = 1; 


es conf ls(LASER MODE ON, THRESHHOLD, WIDTH, NUMDATA, AVG); 
/* conf_cp(1); */ /* =-=-= doesn't work with Ndirect.o */ 
posDataRequest (POS SONAR) ; // Just get the sonar data for 
the SCout 
if (posDataCheck() != (POS SONAR)) { // Jast check for the 


sonar data for the SCOUT 
cout << "\nMERROR: Could not set up pose into for sensors, ya 
exre(= 1); 


} 


// END SCOUT THESIS CHANGE 


void robot: :move(int speed, int angle) 


{ 


// Relative move of <speed>/10 inches forward while turning 
// base and turret by <angle>/10 degrees (+ = ccw, - = cw) 


ine ve luspeed,. vel angic,; // Velocity commands 


vel speed = limit (speed, ROBOT MIN SPEED, ROBOT MAX SPEED); 
vel angle = limit(angle, ROBOT MIN TURN, ROBOT MAX TURN); 


// BEGIN SCOUT THESIS CHANGE 
SCouu vin vel speed, vel angie), 


// scout_pr(speed, angle); 
} 


void robot::fwd(int speed) 


{ 


// Relative move of <speed>/10 inches forward 


// TEMP SCOUT FIX- change pr cmds to vm cmds and comment out the wait 
scout vm(speed, 0); 

if Masti, 1 °0, 25); TEMP FIX - comment out the wait 

} 


VOL": TOPOE; SEurM inte angie) 


{ 
// Turn base and turret by <angle>/10 degrees (+ = ccw, - = cw) 


// TEMP FIX - change pr cmds to vm cmds and comment out the wait 
scout _vm(0, angle); 

TL ws(1l, 1, 0, 5); TEMP FIX - comment out the wait 

} 


// END SCOUT THESIS CHANGE 


VOld TODOE+:SEl Origin heré{vo1d) 
{ 


// Define current position and orientation as origin 
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573 
574 
B/D 
576 
B/ / 
578 
579 
580 
581 
582 
583 
584 
585 
586 
587 
588 
589 
590 
591 
SoZ 
593 
594 
59S 
596 
BY / 
598 
Boo 
600 
601 
602 
603 
604 
605 
606 
607 
608 
609 
610 
611 
612 
613 
614 
615 
616 
517 
618 
619 
620 
621 
622 
623 
624 
625 
626 
627 
628 
629 
630 


/* 


dp(0, 0); 
dale) 27 7 


State 34] 
State[ 35] 


Origin x 
origin ¥ 


ume =e 


x = 0; 
Ver Oe 
theta = 0; 


SGutes<.  selLting OFlgin CO { << Origin x <<) << Origin y <<.) 2” 


<< endl; 


} 


WOU GODSC..Set vOrigin TOe(aniE Oo xX) ine oO. y) 


{ 


// Define new origin relative to current origin 


origin x += 0 x; 
Sr igin yy t= Oy, 


x = 0; 

y= 0; 

theta = 0; 

COUR ) Seuling Of gin, Coit) <- origin x << “5 << ori gin eace 
<< endl; 


} 


int robot::theta2sensor(double theta) 


{ 


} 


// Convert angle to sensor index 
int sensor; 


sensor — (int) (theca * 10.0} / SENSOR SER; 
return(sensor); 


double robot::sensor2theta(int sensor) 


{ 


// Convert sensor index to angle 
double theta; 


theta = (double) (sensor * SENSOR SEP) / 10.0; 
return (theta); 


int robot::sensor_wrap(int index) 


// Wrap index to [ 0..NUM_ RANGE] 
int windex; 


windex = wrap(index, NUM RANGE); 
retumn(windes).; 
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631 
632 
633 
634 
635 
636 
637 
638 
639 
640 
641 
642 
643 
644 
645 
646 
647 
648 
649 
650 
651 
652 
653 
654 
655 
656 
657 
658 
659 
660 
661 
662 
663 
664 
665 
666 
667 
668 
669 
670 
671 
672 
673 
674 
675 
676 
677 
678 
679 
680 
681 
682 
683 
684 
685 
686 
687 
688 


} 


int robot: : check etear(ine “cer, ane waicth, ine dist) 

{ 
// Return 1 if all range sensors in an are <width> x 2 sensors wide 
// centered around sensor <ctr> return greater or equal to <dist>, 
// 0 otherwise. 


int Lefe, i1ghe; 
LINES ese. 


left = sensor wrap(ctr + width); 
right >= sensor wreap(cEr = width); 


Min dist = Tange.min(rigne, plore), 


te (Min dist -— dis.) 7 
recur iis: 
} 
else { 
return(0O); 
} 
} 


void robot: : shutdown (void) 
{ 

SONab Orr); 

if OLE (}% 

laser off(); 


} 


IM: BRODOGE, ;mOVe EO xy (1Me. Cx, Ine icy) 
{ 


// Move robot to <x, y> (world coords, tenths of inch) 


Ime dx, «cy; // Difference between current and desired 
positions 
double dist; // Distance to destination 


double angle; // Bearing to destination 
double mturn; // Turn command 


// BEGIN SCOUT THESIS CHANGE 
scout_vm(0, 0); 
// END SCOUT THESIS CHANGE 


sp(MOVE_TO SPEED, DEFAULT TURN RATE, 0); // TEMP FIX for SCOUT 

update (); 

cout << “current position (" << x << ™, ™ << y << ") : destinaesen 
| aa Cy ae 4 << ey <<") << enc 

ox = Cx = x; 


Gy = Cy = Vy; 
dist = hypot((double) dx, (double) dy); 


if (dist == 0.0) { 
angle = 0.0; 
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690 
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692 
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694 
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696 
697 
698 
699 
700 
701 
702 
703 
704 
705 
706 
707 
708 
709 
710 
711 
712 
713 
714 
#15 
716 
717 
718 
719 
720 
1pAl 
o22 
723 
724 
725 
726 
g27 
728 
n29 
730 
731 
p32 
733 
734 
735 
736 
ay 
738 
739 
740 
74] 
742 
743 
744 
745 
746 


} 
else { 
angle = atan2((double) dy, (double) dx) * RAD2DEG; 


} 


cout << "distance =" << dist << " : angle =" << angle << endl; 


while((dist > MOVE XY MAX ERROR) && (touch.max(0, NUM_TOUCH - 1) == 


Oey 
if (dist > MOVE XY MAX DIST) { 


Couc. << “Destination oo far (“<< dust 7 20.0. <<" anenes) © 
<< 
endl; 
return(0); 
} 
mturn = (int) (angle _sgn_diff(angle, (double) theta / 10.0) * 
POO es 


// BEGIN SCOUT THESIS CHANGE 
// TEMP FIX - change pr cmds to vm cmds and commnent out the ws cmds 
Seouc wa(0,- (ant) meuen) | 


yf Weick Ly Oy 100) TEMP FIX - comment out wait 


SCOuUC vn ( Une) “aise, 0) 7 
Ja wstl. 1, 0, 100)> TEMP FIX - comment out wait 


// END SCOUT THESIS CHANGE 


update (); 

Cout << “Current. position (" <<\x <<", “sey <<) adeseinarton 
| GO ex aa a 2 ee ey << ee end: 

dx = “cx = x; 

dy = cy - y;: 


dist?’— hypoet( (double) dx. double) dy; 
lt -(aiste—-— 10.0 a4 
angle = 0.0; 
} 
else { 
angle = atan2((double) dy, (double) dx) * RAD2DEG; 
} 


cout << "distance =" << dist << " : angle =" << angle << endl; 


} 
Sete deran ttn velLoci Ey): 


return (1)? 


} 


void robot::face angle(int angle) // Desired angle (1/10 degree) 
{ 


// Turn robot to face <angle> accurately 
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747 
748 
749 
750 
Jo 
752 
753 
754 
5 
750 
oa 
758 
foo 
760 
761 
762 
763 
764 
765 
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767 
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775 
776 
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778 
19 
780 
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786 
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792 
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804 


int dtheta; // Difference between current and desired angle 
cout << "Facing angle <" << angle << ">" << endl; 
// BEGIN SCOUT THESIS CHANGE 


scout _vm(0, 0); 
sp (DEFAULT SPEED, FACE TURN RATE, 0); // TEMP FIX for SCOUT 


update (); 
dtheta = (int) 
(angle _sgn_diff((double) angle / 10.0, (double) theta / 10.0) * 
EOS Oa, 


while(dtheta != Q) { 
cout << "current angle =" << theta << " : turn =" << dtheta << 
endl; 
// TEMP FIX - change below to vm vice pr and comment out the wait 
scout _vm(0, dtheta); 
// ws, 1, 0, 200); TEMP FIX comment out the wait 


// END SCOUT THESIS CHANGE 


update (); 
dtheta = (int) 
(angle _sgn_diff((double) angle / 10.0, (double) theta / 10.0) * 
LO2 oe: 
} 


cout << "Alignment complete." << endl; 


set _default_velocity(); 
} 


vVOld)Hobot:*iace sangle fasiiint. angle) // Desired angle (1/10 degree) 


{ 
// Turn robot to face <angle> quickly 


int dtheta; // Difference between current and desired angle 


dtheta = (int) 
(angle sgn _diftt((double) angle / 10.0, (double) theta / 10,0} 
LOO: 


// TEMP FIX for SCOUT to line below 
// cout << "face angle fast(" << angle << "}) ; scout_vm(0, " << )dumeea 
[f/f <<" )" << -eeai, 7) SMe Pix fer Scour 


// BEGIN SCOUT THESIS CHANGE 

// TEMP FIX - change pr cmds to vm cmds and comment out the wait 
Sseout vm(0, dthera) ; 

ff Ves( tel, 0,2re)> TEMP FIX - comment out the wait 

// END SCOUT THESIS CHANGE 

} 


void robot::face angle cont(int angle) // Desired angle (1/10 degree) 
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827 
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829 
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834 
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{ 
// Taem robot to face <angle> quickly, without stopping 


int dtheta; // Difference between current and desired angle 
dtheta = (int) 


(angle sgn_diff((double) angle / 10.0, (double) theta / 10.0) * 
111 Oe @ lp ee 


Cour << “face anole conti woo angle << ") : pr(0, ” <<) dthera <a |” 
<< dtheta << ")" << endl; 
if ((dtheta < -MAX CONT TURN) || (dtheta > MAX CONT _TURN)) { 
yey, mv(MV_VM, 0, MV_PR, dtheta, MV_PR, dtheta); // TEMP FIX - comment 
thas line out 
mv(MV_VM, 0, MV_PR, dtheta, MV_IGNORE, 0); // TEMP FIX - try to fix 
SCOUT problem 
ws(1, 1, 0, FACE CONT WAIT); // wait for both wheels to stop for 
SCOUT 
} 
else { 
Va mv (MV TGNORE U7 My sek, -dthera,, MVIPR s dehercaji: 7/7 EME ve ee 
comment this line out 
mv (MV IGNORE, 0, MV_PR, dtheta, MV_IGNORE, 0); // TEMP FIX = try 


to fix SCOUT problem 


// BEGIN SCOUT THESIS CHANGE 
// do not need this next routine because there is no separate 
7/ turret to align on the SCOUT 
// leave as is because call to turret_align has been commented out 
// in agent.cc 
void robot: :turret_align(void) 
{ 
// Align turret with base 


iMneeturre:; // Turret angle 
Int vatheta; // Difference between turret and base 


Scour yvmncG, 0). 


sp(DEFAULT SPEED, FACE TURN RATE, 0); // TEMP FIX for SCOUT 
update(); 
turret = State 37]; 
dtheta = 0; f/f £ake*it tor. SCOUT 
// dtheta = wrap(actual theta - turret, -1800, 1800); 


while(dtheta != 0) { 
scout vm(0, 0); 7 / PI EMP Eeh Lor eocesT 
WS CO gmt: VOieechOCDN. 


update (); 

turret = State 37]; 

Genet AS bap actua bat nerag—— tur cet, 613007 G00), 
} 


// END SCOUT THESIS CHANGE 
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set default _velocity(); 
} 


Void L.obot: :move mellimt x, ane 4) 


{ 


// Relative Cartesian move to <x, y> (world coords, tenths of 
inches) 


int sold angle; 
int move_angle; 
IMte Move isk: 


update (); 
old angle = Statef[ 36] ; 


move angle = (int) (atan2((double) y, (double) x) * RAD2ZDEG * 1070)7 
move dist = (int) hypot((double) x, (double) y); 


pace sangle(move angle); 


sp(MOVE TO SPEED, DEFAULT TURN RATE, 0); // TEMP FIX for SCOUG 
// BEGIN SCOUT THESIS CHANGE 


// TEMP FIX - change pr to vm and comment out the wait 
scout vm(move dist, 0); 
ie wsii- “1. 07100). TEMP FIX - comment out the wait 
// END SCOUT THESIS CHANGE 
set default velocity (); 


face angle(cld angle); 
} 


void robot: :wait_start (void) 


{ 


// Wait for the robot to start moving (any motor) 


gsi); 

while ((State[ STATE VEL TRANS] == 0) && 
(State[ STATE VEL STEER] == 0) && 
(State[ STATE VEL TURRET] == 0)) { 


Gs, 
} 
} 


12 


O COONAN BW DN 


APPENDIX G. FRONTIER-BASED EXPLORATION CODE — AGENT.H 


This appendix contains the header file for the routine that controls the robot’s exploration 
behaviors. 
/* 
agent.h 
Header file for agent class 
=o) 


#ifndef AGENT_H 
#define AGENT H 


#include "drand.h" 

#include "irand.h" 

Pimciude- "robot .h"” 

#include “place _net.h" 

#include "arb.h" 

#include "control _panel.h" 

Pome lude “bar graph. h” 

#include "mobstacle.h" 

#include "comm++.h" 

#include "comm.h" 

Pomclide - frontier. h” 

#include "path.h" 

// BEGIN SCOUT THESIS CHANGE 

// These are the conversion macros from Nomadic that accept the steering 
and 

// translation values as used for the Nomad 150 and 200 and calculate 


the 


Vy da tterential—drive axis values for the Nomad Scout. 


#define ROTATION CONSTANT 0.118597 /* inches/degree (known to 100 
ppm) */ 
#define RIGHT(trans, steer) (trans + 


(init) ((tleoag)steer* ROTATION CONSTANT)» 


#define LEFT (trans, 


steer) 


(trans - 


(int) ((float) steer* ROTATION CONSTANT) ) 


#define scout _vm(trans, steer) vm(RIGHT (trans, steer), LEFT(trans, 
Steer), QO) 
#define scout pr(trans, steer) DE(RIGHT (trans, steer), LEFT (trans, 
steer), 0) 


// END SCOUT THESIS CHANGE 


/7/ Meteor 


Const. in 


control parameters 


tL OPE LD RES =140; 


195 


109 
110 


const 
const 
const 
const 


Conse 
const 
const 
const 
const 


// Random turn to escape stasis 


double 
double 
double 
double 


int. TURN, RES. = 


double 
double 
double 
double 


SPEED MIN 
SPEED MAX 
SPEED DEF 
SPEED NOIS 


o 32 
TURN MIN = 
TURN MAX = 
TURN DEF = 
TURN NOISE 


const double RAND TURN 


// Speed arbitrator 


const 
const 
Const 
const 
const 
const 


int SPWIN X = 
int SPWIN Y = 


570 
460 


int SPWIN WIDTH = 
int SPWIN HEIGHT 


double 
double 


SPWIN MIN 
SPWIN MAX 


E 


° 
c 


° 
v 


e 
f 


— 
a 


= 0020; 


LOO; 
Ga 
= 50; 


-180.0; 
eC. oe: 
Ome 


ors 


LOA0 


L7S- 


5) 
=20'./0; 
20. 


window parameters 


as 
if. 
ia 
i 


// Turn arbitrator window parameters 


Conse 
const 
const 
Const 
Const 
const 


Iie 
Dre 
anit 
Lc 
double 
double 


TUWIN X = 
TUWIN Y = 
TUWIN WIDTH = 
TUWIN HEIGHT 


570 
540 


TUWIN MIN 
TUWIN MAX 


// Power constants 


const 


double 


const double 


Const 
const 


double 
double 


[* KK KK kk kk 


// Bump halt 


CPU FULL VOLTAGE 
CPU_DANGER_VOLTAGE 


MOTOR FULL VOLTAGE 
MOTOR DANGER VOLTAGE = 


const int BUMP_SLEEP = 


// Recoil 


const 
const 
const 
Const 
const 


double 
double 
double 
double 
double 


RECO SPeeD 
RECO SPEED SIGMA 
45.0: 


RECOIL TUR 


e 
f 


° 
A 


eG 


N 


L75- 


SOE 
=20.0; 
20.07 


e220. 
= 11.0; 


=e ae 
dele: 


= 100.0; 


RECOIL TURN SIGMA 


RECOIL WT 


// Maintain alignment 


Loe 0: 


= 25.0; 


LO.U- 
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X—-COOrd Of Op 
V=COord of ert side 
Window width 
Window height 
// Minimum vote total 
// Maximum vote total 


x-COOrdad OF top 
V=GOOrd Of leit side 
Window width 
Window height 
// Minimum vote total 
// Maximum vote total 


GO; 


BEHAVIOR CONSTANTS ***** ee ee / 


// Number of seconds to sleep 


ee ee ee ee 
fem mh ph fet feed fees ped 
“JON On BN 
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120 
121 
122 
2 3 
124 
125 
126 
127 
128 
129 
130 
131 
132 
133 
134 
135 
136 
137 
138 
39 
140 
141 
142 
143 
144 
145 
146 
147 
148 
149 
150 
151 
2 
153 
154 
155 
156 
BO] 
158 
159 
160 
161 
162 
163 
164 
165 
166 
167 
168 


const double MAX BASE TURRET DEV = 1.0; 


// Advance 
const int ADV_SLOW_ DIST = 60; 
const int ADV_ STOP DIST = 6; 


Const int ADV_ PERS SLOW_ DIST = i2: 
Genst int ADV_ PER_ STOP DIST = 4 
const double ADV_ SPEED = 75.0; 

const double ADV_ PER SoPEED = 20.0; 
const double ADV_ SPEED_ SIGMA = 10.0; 
const double ADV SPEED WT = 52.0; 


// Bdvance slow 

Sense int ADV SLOW STOP DIST = 5; 

const double ADV_ SLOW SPEED = 202.0" 
eonst double ADV SLOW SPEED SIGMA = 5.0; 
const double ADV SLOW_SPEED WT = 5.0; 


// Corridor advance 


const int CORRIDOR_SPEED = 25; 
@otst int CORRIDOR SPEED WIDE = 73; 


// Maintain heading 


eonst double ME TURN SIGMA = 45.0; 
const double MH TURN WT = 1.0; 


// Maintain transit heading 


Conse, double MIR TURN SIGMA =.45..07 
Gonste double Min TURN wir] 1.0; 


// Avoid 

Gonst. int AVOID DIST = 36; 

const double AVOID TURN SIGMA = 22.5; 

Const double AVOID WI FACTOR = 6.0; 

7/7 Transitvevoid 

const double TRANSIT AVOID TURN SIGMA = 10.0; 
J SATO toe Dias 

const int AVOID BIAS DIST = 10; 

const double AVOID BIAS _ANGLE = 45.0; 


const double AVOID | BInce _SIGMA ASO, 
const double AVOID BIAS WT = 1.0; 


// Follow wall 


const int FOLLOW ABORT = 20; 

const int FOLLOW MAX ALIGN DIST = 40; 
const int FOLLOW_STOP DIST = 20; 

const double FOLLOW TURN FACTOR = 0.2; 


1S 


169 
170 
171 
172 
lv 
174 
175 
176 
177 
178 
179 
180 
181 
32 
183 
184 
185 
186 
187 
188 
189 
190 
19] 
197 
193 
194 
195 
196 
197 
198 
199 
200 
201 
202 
203 
204 
205 
206 
207 
208 
209 
210 
211 
ZZ 
213 
214 
215 
216 
Za 
218 
Zo 
220 
22 
222 
223 
224 
Bes 
226 


const double FOLLOW TURN SIGMA = 5.0; 
const, double FOLLOW Wi = 2.0, 


// Maintain distance 

COnsSt- 12nc DESIRED Dist = 10; 

const double MD TURN FACTOR = 0.2; 
const double MD TURN SIGMA = 3.0; 
€onse double MINT = 4.0; 

// Follow path 

Const, double NAY MEN ACT = 40.0: 
const double NAV_SIGMA = 45.0; 
const double NAV WI = 35.0; 


// Goal orient 


const double GOAL SIGMA = 45.0; 
const double GOAL WT = 5.0; 


// Goal -corridor orient 
const double GOAL CORRIDOR_NOISE = 5.0; // Noise in turn angle 
// Center home 


GOnst ine CENTER SSEEED = 307 
Gense Gouble CENTER ERR SIoRESH — 90-01; 


// Angle localization 

const int ANGLE LOC STEP = 10; 

const int ANGLE LOC NUM STEPS = 10; 

const int ANGLE LOC SLEEP = 1; 

[RR KKK KR KK SEQUENCER CONSTANTS Hk ke ke ek ke / 


// 22.5 degrees between sonars 


const int SONAR SWEEP WIDTH = 22; 
// Sonar sweep speed 
const int SONAR SWEEP SPEED = 20; 


// Sonar sweep step (degrees) 

const int SONAR_SWEEP STEP = 2; 

// Sonar sweep pause between steps (microseconds) 
const unsigned int SONAR_SWEEP PAUSE = 100000; 

// Laser sweep step (degrees) 


const int LASER SWEEP STEP = 5; 
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D2] 
228 
229 
230 
231 
D532 
233 
234 
235 
236 
g3/ 
238 
239 
240 
241 
242 
243 
244 
245 
246 
247 
248 
249 
250 
251 
252 
253 
254 
255 
256 
ED/ 
258 
259 
260 
261 
262 
263 
264 
265 
266 
267 
268 
269 
270 
271 
272 
273 
274 
275 
276 
Dj 7 
278 
zZ19 
280 
281 
282 
283 
284 


// Laser-limited sonar sweep speed 

const int LLS TURN RATE = 150; 

// Identification confirmation sequence (inches) 

const double MAX CONFIRM Dist = 1-0; 

// Local navigation sequencer tolerance (inches) 

const double LOCAL NAV TOLERANCE = 18.0; 

// Local navigation maximum timesteps for stall 

Gense int STALE TIMEOUL = 20; 

// Angle above which local navigation turns robot in place 

const double LOCAL TIP_ANGLE = 90.0; 

eee hae CONTROL INTERFACE PARAME TRRG =" = 24x sex) 

const int NUM_CMD = 2; // Number of command outputs 
enum { SPEED, TURN }; // Command indexes 

// Behavior modes 

enum { EXPLORE MODE, EXPLORE LLS MODE, NAVIGATION MODE, TEST MODE } ; 
// Control commands 


enum { CMD EXPLORE, 
CMD — NAV, CMD NAV _ KBD, CMBSSTOPR, CMD _ SAVE, CMD | LOAD, 
CMD REDRAW, CMD | BUILD. GRID, CME SAVE | GRID, CMD _ LOAD GRID, 
CMD | _GRID_ LDENL, ~ CMD GRID, CMD CLEAR, CMD _ CLEAR _ ROBOT, 
CMD _ _ SONAR _ SCAN, CMD | | SONAR _ SWEEP, CMD SONAR _ SWEEP _ABS, 
CMD CLEAR SONAR, 
CMD _LASER_SCAN, CMD _LASER_SWEEP, CMD LASER SWEEP ABS, 
CMD CLEAR LASER, 
CMD_LLS_ SCAN, CMD _LLS_ SWEEP, CMD _LLS SWEEP ABS, CMD CLEAR LLS, 
CMD _GRID_ UNDO, CMD _ CENTER, CMD _ PEACE MAP, 
CMD: PLACE _ IDENT, CMD _ PLACE _ GRID, 
CMD | LOCAL” NAV, CMD ADD_ PLACE, CMD EDIT PLACE, 
CMD ADD_ EDIT _LINK, CMD _ DELETE _LINK, 
CMD _ CLEAR _ GLOBAL, CMD > SAVE _GLOBAL, CMD LOAD GLOBAL, 
CMD _ DISPLAY _GLOBAL, CMD | GLOBAL _ UNDO, CMD _ INTEGRATE GRID, 
CMD _ FIND | FRONTIERS, CMD _ DISPLAY _EDGES, CMD _ DISPLAY _ FRONTIERS, 
CMpE ECoTes FRONTIER, CMD __ UPDATE _NAV _GRID, CMD _ DETECT _ _CORRIDORS, 
CMD _ CONNECT Seb eeUDs SEND _ Cie _GRID, 
CMD BUMP, 
CMD INIT COMM, CMD SEND MSG, CMD RECEIVE MSG, 
EMDZEXTIT | 
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// Control window graphic parameters 


Lo 


285 


const int 


CON_NUM_CMD = 


CMD EXIT + 1; 


// Number of command buttons 


286 

287 const int CON WIN LEFT = 850; // x-coord of left side 
a const int CON WIN TOP = 0; // x-coord of top 
290 // Number of button columns 

291 const int CON COLS = 2; 

oe 

293 // Number of button rows 

294 const int CON ROWS = (int) ((double) CON NUM CMD / 2.0 + 0.5); 
295 

296 const int CON BUTTON WIDTH = 150; // Button width 

ae const int CON BUTTON HEIGHT = 25; // Button height 

299 const int CON LAB WIDTH = 130; // Label width 
300 // (Must be less than button width) 
301 const int CON_LAB HEIGHT = 10; // Label height 
ae // (Must be less than button height) 
0 

Ge // Evidence grid window screen boundaries 

30 

306 const int EGWIN LEFT = 420; 

307 const int EGWIN TOP = 0; 

308 const int EGWIN RIGHT = 932; 

309 const int EGWIN BOTTOM = 512; 

310 

ap // Evidence grid window world coordinate boundaries 

3 

313. const int EGWIN WC LEFT = -3000; 

314 const int EGWIN WC BOTTOM = -3000; 

315. const int EGWIN WC RIGHT = 3000; 

316 const int EGWIN_WC TOP = 3000; 

Sly 

aS // Navigation grid window screen boundaries 

320 const int NAV_WIN LEFT = 420; 

321 const int NAV_WIN_TOP = 0; 

322 const int NAV WIN RIGHT = 932; 

323 const int NAV WIN BOTTOM = 512; 

324 

Boe // Navigation grid window world coordinate boundaries 
327 const int NAV WIN WC LEFT = -6000; 

328 const int NAV WIN WC BOTTOM = -6000; 

329 const int NAV WIN WC RIGHT = 6000; 

330 const int NAV WIN WC TOP = 6000; 

33] ne 

oa) sf 

333, const int NAV WIN WC LEFT = -3000; 

334 const int NAV WIN WC BOTTOM = -3000; 

335. ~const int NAV WIN WC RIGHT = 3000; 

336 const int NAV WIN WC TOP = 3000; 

os) tf roe 

338 

a // Global evidence grid window screen boundaries 

341 const int GLOBAL WIN LEFT = 0; 

342 const int GLOBAL WIN TOP = 0; 
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const int GLOBAL WIN RIGHT = 1024; 
const int GLOBAL WIN BOTTOM = 1024; 


/* 

const int GLOBAL WIN LEFT = 0; 
const int GLOBAL WIN TOP = 0; 
const int GLOBAL WIN_RIGHT = 512; 
const int GLOBAL WIN BOTTOM = 512; 


ee 

// Evidence grid window world coordinate boundaries 
const int GLOBAL WIN _WC_LEFT = -6000; 

const int GLOBAL WIN WC_BOTTOM = -6000; 

const int GLOBAL WIN WC_RIGHT = 6000; 

const int GLOBAL WIN WC_TOP = 6000; 

/* 

const int GLOBAL WIN WC_LEFT = -3000; 

const int GLOBAL WIN _WC_BOTTOM = -3000; 

const int GLOBAL WIN WC RIGHT = 3000; 

const int GLOBAL WIN WC TOP = 3000; 

x / a as 

// Color of robot in global window 

static char GLOBAL ROBOT COLOR[ STRLEN] = "Blue"; 

// Color of contact marker in global window 

Static char CONTACT COLOR STRLEN]) = "Read"; 

// Size of contact marker in global window 

Const int CONTACT MARK SIZE — 50; 

[RR KK KKK KK FRONTIER CONSTANTS Kk  / 

// Maximum number of frontiers 

const int MAX FRONTIERS = 1000; 

// Radius of neighborhood around visited frontier (1/10 inch) 
eonse, double Visit RADIUS j= 6020, 

// Radius of neighborhood around inaccesible frontier (1/10 inch) 
eonst. doubles INACy RADIUS) =—120.0; 

j//>Mazimum mumber sot “colors ’for blob eoloring 

const int MAX COLORS = 1000; 


// Number of colors to display 


const int DISPLAY COLORS = 16; 
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// Radius of region centroid marker 
const double CENTROID MARK RADIUS = 75.0; 
// Minimum region size 


const int MIN REGION SIZE = 6; 
// Conese mme MIN REGION SIZE — 1; 


// Maximum frontier distance 
const double MAX DIST = 100000.0; 


// Frontier edge color 


static char EDGE COLOR STRLEN] ered. 
i/ Color fable 


Static char color table! DISPLAY COLORS] STRLEN) F—4 
"Blue", "Green", "Gold", "Red", 


"SkyBlue", "LimeGreen", "Orange", "Magenta", 
 Roval Blue, “Gyan, “lighntCoral®,. “Vroler’, 
"SteelBlue", "Aquamarine", "Purple", "Turquoise" } ; 


// Color conversions for robot window 


State Ine. nopet Color, DISPEAY COTORS|, = 4 
lig. 1 Ae? ee 

~ Zaher 

nk Sy eels, 

gy oye 220 al: 


t 


m GW Po 
io © ~) 


’ 
[A ee NA TAEGAL TON CONSTANTS * ex * ee 2a 

// Distance to retreat on bumper contact (1/10 inch) 

const int BUMP RECOIL = 20; 

// Speed of bump recoil 

Gonst int BUMP RECCTE SPEay = 20; 

// Search status codes 

const int SEARCH SUCCESS = 0; 

const int SEARCH FAIL = 1; 

const int SEARCH TIMEOUT = 2; 

// Maximum number of cells to search 

const int SEARCH MAX CELLS = 10000; 

// Maximum number of obstacle cells allowed in path region 
const int MAX OBS COUNT = 2; 


// Color of path in Grid/qlobal window 
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static char PATH COLOR STRLEN] = "Red"; 
static char OPT PATH COLOR[ STRLEN) = "Blue"; 
static char TRAV PATH COLOR STRLEN] = "Red"; 


// Color of path in robot window 


const int ROBOT PATH COLOR = 16; // Red 
const int ROBOT OPT PATH COLOR = 2; // Blue 
const int ROBOT TRAV PATH COLOR = 16; // Red 


// Waypoint lookhead window (# waypoints) 

const int WAYPOINT WINDOW = 5; 

// Number of waypoints between LLS sweeps during navigation 
const int NAV _LLS SWEEP_INTERVAL = 10000; // Never 
[RRR RK RK eK CORRIDOR CONSTANTS he ke ee i ee / 

// Number of sensors to either side of sensor to check 
const int CORRIDOR _SPAN = 3; 

// Amount of forward space needed to be clear 

const int CORRIDOR FWD_RANGE = 12; 

// Amount of space needed on sides of robot 

const int CORRIDOR SIDE CLEARANCE = 6; 

// Amount of forward space for wide corridor 

const int CORRIDOR _WIDE FWD RANGE = 48; 

// Amount of side space for a wide corridor 

const int CORRIDOR WIDE SIDE CLEARANCE = 24; 

// Maximum deviation between corridor angle and goal bearing 
const double CORRIDOR MAX DEVIATION = 90.0; 

//- COLOLr (Of “corridor inv globaly window 

static char CORRIDOR COLOR[ STRLEN] = "Blue"; 

static char CORRIDOR WIDE COLOR STRLEN) = "Green"; 
static char CORRIDOR SELECT COLOR, STRLEN) = "Red"; 


static char CORRIDOR SELECT WIDE COLOR STRLEN] = "Gold"; 


//2COlVOr Of SCOrrider-in robot window 


Gems tsimt= CORRIDOR GoLORe ROBOT — 125 // Blue 

const int CORRIDOR WIDE COLOR_ROBOT = 6; // Green 

const int CORRIDOR SELECT COLOR _ROBOT = 14; // DeepPink 
const int CORRIDOR SELECT WIDE COLOR ROBOT = 11; // Yellow 
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fRRKRAEE EEX CONTINUOUS LOCALIZATION DECLARATIONS) == 7 2 
// Conmeinuous Localization host 

static char CONTLOC HOST[ STRLEN) = "SunZz6s"; 

// Continuous localization communication channel ID 

const int CONTLOC CHANNEL = 0; 

// Minimum number of occupied cells for usable map 

Const ane. CONTLOC MIN Occ = 0; 

// Exploration relocalization interval (inches) 

const int EXPLORE RELOC DISTANCE = 96; 

// Navigation relocalization interval (inches) 

const int NAV_RELOC DISTANCE = 24; 

fat eee MULE ROBO. DECEARATIONS 47477 occ 7 

// Message indicating new map exists 

static char NEW MAP MSG[ STRLEN] = "NEWMAP" ; 

fret See ee MSC rE LANEOUS DECLARATIONS: 22722725 .7 7 

// Status codes 
const int OK = 0; 
const int ALT = 1; 
const int RETRY = 
const ant. ABORT 
COuSe Inter TIMEOUT = —-1001, 


const int NO PATH = -1002; 
Conse Ine NOeRRON TIERS = "1005, 


I 
J 
— 
© 
© 
© 


// External C functions 

extern "C" wnt abs (ant)? 

extern "C" int sleep(int); 

extern "C" int usleeco (unsigned int); 

extern "C" void exit(int); 

extern "C" void registergrids (Map3D mapl, Map3D map2, double * dx, 
double *dy, double *dt, double *fitness); 

// Number of moving obstacles 

const int NUM MOB = 0; 

// Length of experimental trial (steps) 


//const int TRIAL LENGTH = 10000; 
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const int TRIAL LENGTH = 50000; 
//const int TRIAL LENGTH = 1000000000; // Ran forever 
// Margin for random robot placement 
const int RAND MARGIN = 200; 
class agent { 
HUGE: 
agent (void); 7/7 COnstructor 
void control(void); // Main control loop 
private: 
int bumped{ NUM TOUCH] ; // BUMPER HACK ARRAY 
BObOr a, Vf Controlled ropot 
arbitrator *speed arb; // Speed command arbitrator 
AL Ditraton “turn daub, // Turn command arbitrator 
place net pnet; // Place network 
char apndir[ STRLEN] ; // Name of APN directory 
mobstacle mob list[ NUM_MOB + 1}; // Moving obstacles 
Map3D global grid; // Global evidence grid 
Map3D old global; // Old global evidence grid 
Map3D egrid; // Evidence grid 
Map3D old grid; // Old evidence grid 
Map3D edge _ grid; // Frontier edge grid 
Map3D nav_grid; // Navigatison orid 
int region_map{ GLOBAL X_RES][ GLOBAL Y_ RES] ; // Colored region grid 


int visit[ NAV_X_RES][ NAV_Y_ RES); // Visit map for path planning 


frontier frontiers[ MAX FRONTIERS] ; // List of frontiers 
int num_front; // Number of frontiers 
ErOontier front ViSit| MAX FRONTIERS] ; Li Visited, Sone bers 
int num_visit; // Number of visited frontiers 
Erontier f£Fontyinac| MAX, PRONTIERS|); // Indecessibles frontiers 
int num_inac; // Number of inaccessible frontiers 
int corridor{ NUM_RANGE] ; // Corridor detection array 
int wide corridor[{ NUM RANGE] ; // Wide corridor detection 
array 
CylSensorModelArray sonar_smd; // Sonar sensor model 
CylSensorModelArray sonar clear smd; // Sonar sensor model (clear) 
control panel control window; // Control window 
// bar _graph speed window; // Speed command display 
// bar graph turn_window; // Turn command display 
window * grid window; // Evidence grid window (pointer) 
// window *nav_window; // Navigation grid window (pointer) 
window *global_ window; // Global grid window (pointer) 
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ine Global sterresn; 
ic Ceabe lime rsp tay, 


ofstream *logfile; 


int multi_mode; 
inte.2cont loc mode; 
int behavior, mode; 
int home dist; 


int destin; 


int timer; 

double cpu volt; 
double motor volt; 
double cpu_min; 
double motor min; 


double transit_dir; 


int pause mode; 
inevesl Counc, 


void reset (void); 

int user _ command (void); 
int, 1scan (void); 

void terminate (void); 
void power _check(void); 


// Behavior modes 


void bump test (void) ; 

void manual _ exploration (void); 
conLred 

VOld exp loracicn( void), ; 

void exploration _lls(void); 


void reactive exploration (void); 


// reactively 

void navigation(void) ; 

void navigation _keyboard (void) ; 
(keyboardad) 

woud: Local navigation (void); 
destination point 

void frontier navigate (void); 
centroid 


// Explore uncharted territory 
void multi_exploration(void); 


ae 
Ue 


// 


// Multirobot mode 


if 
Le 
// 
Ve 


a 
Tf 


tah 
Ty 


fa 
Te 
re 
ay 
i 


ay 
i 


Ae 
ie 


la 
a 


set when global grid is displayed 
Flags whether to display robot 


Log fiie 
(O:single,. l-mulem 


Continuous localization mode 
Behavior mode 


// Path distance from home 


Destination index 
Total elapsed time (steps) 


CPU battery voltage 
Motor battery voltage 


// Minimum CPU battery voltage 


Minimum motor battery voltage 


Transit direction 


// Number of cells searched 


Reset position and timer 
Execute user command (if any) 
SGan Tor i1nterrupe 

End session 

Check battery power 


Bumper test 
Map territory under manual 


Explore uncharted territory 


Explore uncharted territory (LLS) 
Explore uncharted territory 
Navigate to destination (mouse) 


Navigate to destination 
// Navigate to local 


// Navigate to frontier 


(multiple trials) 


// Explore uncharted territory reactively (multiple trials) 
void multi reactive exploeration(void); 


// Behavior sequencers 


// Manual exploration sequencer 
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void manual _exploration_seq(void) ; 


// Exploration sequencer 
void exploration _seq(void); 


// Exploration sequencer (laser-limited sonar) 
void exploration _lls seq(void); 


// Reactive exploration sequencer 
void reactive exploration_seq(void); 


int navigation_seq(void); // Navigation sequencer 

void search seq(void); // Search sequencer 

void map_seq(void) ; 7? Moiid local grid 

wold Ceneer Seq(vord): 7/7 Move CO Conver Of current place 


// Local navigation sequencer 
ime, Local Nav Seq(int x, int y¥); // Local destination coordinates 


// Local navigation sequencer for path following 
int path_local nav_seq(path p, if Path to £oliog 
int &waypoint); // Index of next waypoint 


// Local navigation sequencer (continuous motion) 
int local _cont_nav_seq(int x, int y); // Local destination coords 


// Local navigation sequencer (with alternate goal) 
int local nav_seq alt(int gx, int gy, // Goal coordinates 
int ax, int ay); // Alternate goal coordinates 


// Navigate to goal by planning and following path 
int path _nav_seq(double gx, double gy); // World coords of goal 


// Navigate to frontier by planning and following path 
int frontier path nav_seq(int front _index) ; // Frontier index 


// Place identification sequencer 
VOLaOLOenE Seq (vord); 


// Grid identification sequencer 
VOL grid 1denbeseq( void) > 


// Rotate and reset DR angle to match stored range image 
void angle loc _seq(int image[ NUM_RANGE] ); 


// Translate to match stored range image 
void trans _ loc seq(int image[ NUM RANGE] ); 


// Rotate sonar sensors and scan 
void sonar sweep seq(Map3D map) ; 


// Rotate sonar sensors and scan (absolute coordinates) 
void sonar sweep abs seq(Map3D map); 


// Rotate laser scanner and scan 
VO1d. laser sweep seq (Maps) map; 


// Rotate laser scanner and scan (absolute coordinates) 


205 


749 
750 
il 
722 
753 
754 
72> 
756 
qT 
758 
759 
760 
761 
762 
763 
764 
765 
766 
767 
768 
769 
770 
771 
Wiz 
TIS 
774 
> 
776 
cay 
778 
779 
780 
781 
782 
783 
784 
785 
786 
787 
788 
789 
790 
791 
oe 
195 
794 
125 
796 
10 
798 
799 
800 
801 
802 
803 
804 
805 
806 


void laser sweep abs_seq(Map3D map); 


// Laser-limited sonar sweep 
void lls _sweep_seq(Map3D map); 


// Laser-limited sonar sweep (absolute coordinates) 
void lls sweep _abs_seq(Map3D map); 


// Navigate to selected frontier 
int frontier nav_seq(int front_index); // Frontier destination index 


// Behavior sets 


// Behavior set for reactive exploration 
int reactive explore behaviors (void) ; 


int navigation_behaviors (void) ; // Behavior set for navigation 


// Behavior set for local navigation 
Int H6cal~navigar1 On behav tors (int gx, Integy); 


// Benaviors 


[RRR KKK KK eK LOW-LEVEL BEHAVIORS Hk kk ek ee / 


void bump halt (void); // Go limp if bumper touched 

void recoil (void); // Zi touched in forwara hale. move 
backward 

void bump recoil (void); // If bumper contact, recoil away 

void wander (void); // Make ‘small Yvandom turns 

int advance(void); // Move forward unless front is blocked 

int advance slow(void) ; // Move forward slowly unless front is 
blocked 


// Realign turret if it is not aligned with base 
void maintain alignment (void); 


// Avoid nearby obstacles 
VOU <avyord (void); 


// If front is completely blocked, bias avoidance toward the left side 
VOl1d-ayord bias -lebe(vord):, 


// If front is completely blocked, bias avoidance toward the right 


side 


vVO1d avoid bias right (void); 


// Maintain current heading 
vyold Maintain neading (void); 


void veer (void); // Veer away from potential collisions . 
void follow _wall_ right (void); // Align with right wall 
void follow wall _ left (void); // Align with left wall 


// Maintain desired distance from right wall 
void maintain distance right (void) ; 
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// Maintain desired distance from left wall 
yOidemaintain Gistance left (void); 


// Turn toward goal 
VOrd goal Oriene(int.9gx, int gy), 


JERE Ree NAVIGATION BEHAVIORS kkk kk KR KK / 


int follow_path(void) ; // Turn to follow path 
int “detect: dest (int destin); // Detect arrival at 
destination 


// Low-level commands 


void set cetaults(vord): // Set default command values 
void update (void); // Update robot state and evidence grid 
void execute (void); // Send commands to robot 


// Move obstacles 
void move obstacles(void); 


JV/eDelete obstacles 
void del obstacles (void); 


// File access commands 


void save net(void); // Save network in file 
void load_net (void); // Load network from file 


[xe ee eee LOCALIZATION FUNCTIONS ***** #4 / 


// Compute difference between image and range input 
double compute range err(int image[ NUM_RANGE], vector rinput); 


// Compute translation vector between expected and actual position 
void trans_loc_vector(int image[ NUM RANGE), int &dx, int é&dy); 


Pott ere EY LD DENCE. GRID: BUNCTIONS @*** +24 ees 
// Display evidence grid in X window 
void grid_display(window *win, // Window pointer 


Map3D map) ; // Evidence grid 


// Display global evidence grid in X window 
vold grid display global (Map3D map); // Evidence grid 


// Display local grid for place 
yOld Gisplay place Guard vod); 


// Display edge segments detected in evidence grid 
void grid display edges(int grid{ GLOBAL X _RES)[ GLOBAL Y_ RES] ); 


// Display regions detected in evidence grid 
void grid display regions(int grid{ GLOBAL X RES)[ GLOBAL Y_ RES] ); 


// Display robot in window 


void display robot (window *win, // Window 
EME yo ite Vy, / / SRODOE COSTE TOn— (1/10 sine) 
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int theta, // Robot heading (1/10 degree) 
int turret); // Robot turret angle (1/10 degree) 


fRRERE EE CE FRONTIER FUNCTIONS He ee ee ef 


//eepy Brontier <f2> to frontier <L£1> 
void Erontier copy (frontier Gil, frontier, 22); 


// Find frontiers in global grid 
void find_frontiers(void); 


// Find frontier edges in <raw> grid and store them in <edge> grid 


void find frontier edges (Map3D * raw, // Raw evidence grid 
(porncer} 
Map3D *edge, // Frontier edge grid 
(pointer) 
double height); // Z-axis of edge plane 


// Find frontier regions in <edge> grid and add new frontiers 
void find frontier regions(Map3D edge, // Frontier edge grid 
double height); // Z-coord of edge plane 


// Segment <grid> image into regions in <color> using spreading 
activation 


void spread segment (Map3D grid, // Uncolored grid 
int color[ GLOBAL X_RES][ GLOBAL Y RES], // Colored grid 
double height) ; // Z-coord of edge plane 


// Print colored grid cell values 
void print region _map(int grid{[ GLOBAL X RES][ GLOBAL Y RES] ); Wye 
Colored grid 


// Determine size and centroid of frontier regions 
void analyze regions(int grid{ GLOBAL X RES]{ GLOBAL Y RES] ); ay 
Colored? grid 


// Check whether centroid corresponds to previously visited frontier 
// Return 1 if visited, 0 otherwise 
int agent::visited(double cx, double cy); 

// Centroid of new region 


// Check whether centroid corresponds to inaccessible frontier 
// Return 1 if inaccessible, 0 otherwise 
int agent::inaccessible(double cx, double cy); 

// Centroid of new region 


// Return index of unvisited, accessible frontier closest to (x, y) 
// Feturnme-l1t no such frontier exists 
int closest _frontier(double x, double y); 


// Mark region centroids in evidence grid window 
void display region _centroids(double cx, // Display center x- 
COOrd 
double cy); // Display center y-coord 


// Mark region centroids in robot window 
void display robot_region_centroids (void) ; 
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// Check whether cell (x, y) is part of frontier <front_index> 
int: check frontier celil(ink x, Ine iy; // Cell index 
int front index) ; // Frontier index 


PUR SR REAR KARA RE he NAVIGATION FUNCTIONS RRR RR RRR RR Ke ek Ke kek 


J/ Move forward if Eront corridor is clear 
WOlLd cOrridor advanee( void); 


// Turn toward clear corridor closest to goal bearing 
Told Goal COLrrIaGT Orlent line gx, Ine Gy)? 


// Update navigation grid based on global grid 
void update nav_grid(void); 


// Plan path to goal location (return 1 if path found, 0 otherwise) 
int path _plan(double wx, double wy, // Moric coouds Of edoa |! 
path é&nav_ path); // Navigation path (optimized) 


// Plan path to goal location (return 1 if path found, 0 otherwise) 


amt frontier path plan(double wx, double wy, // World coords o£ goal 


int front_index, // Frontier index 
path &nav_path); // Navigation path 


// Print all cells on path 
VOL"Gd Print path (path p); 


// Draw path in window 


void display path(path p, // Path 
enar *pcolor, // Pach color 
window *win); // Window 


// Draw path in robot window 


void display path_robot (path p, // Path 
iit. PCOlar); //-~ Path color 
// shine Cache aron “Sk, (SV) sco. gx, Gy) 
ime vtec: Pabh(iie Sx peas sy, J) Starteacel | 
TnenGx Int say, // Goal cell 
path &p); // Path 


(7) Pamcdeeathe fromal(sx,5SV)  bost Ox ~oyiOr any epOilnt ony irons ler 


<iront andex> 


IEA cVONeLOre EMC pati (IMEwsx, «1 Ntesy, §// soeartencoll 
IiEsOx Integy, v7. Goalpcell 
iit eorOnt Si Rdex, // baone ter “index 


path &p); // Path 

// Search cell (x,y) and return search status 
int search cell(int x, int y, // Search cell 

int gx, int gy, // Goal cell 

path &p); // Path 
// Search cell (x,y) while navigating to frontier and return search 

status 

icon gOMedets tseqhell feel dnt ex ality, // Search cell 
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981 
982 
983 
984 
985 
986 
987 
988 
989 
990 
991 
22 
95 
994 
995 


ooT 

998 

999 
1000 
1001 
1002 
1003 
1004 
1005 
1006 
1007 
1008 
1009 
1010 
1011 
1012 
1013 
1014 
1015 
1016 
1017 
1018 
1019 
1020 
1021 
1022 
1023 
1024 
1025 
1026 
1027 
1028 
1029 
1030 
1031 
1032 
O33 
1034 
1035 
1036 
1037 
1038 


int ox. tat oy, // Goal cell 
int front index, // Frontier index 
path &p); // Path 
// Find index of (unvisited) neighbor closest to goal 
int closest neighbor (int x, int y, // CUurpent cel laine. 
iit cx ae oy, // Goal cell index 
iat Snxn ne any) // Next cell index 


// Reverse order of steps on path 
void reverse path(path old path, J//Wnitial oatn 
path &new path); // Reversed path 


// Optimize path by jumping between adjacent path cells 
void optimize _path(path old_path, // Initial path 


path &new path); // Optimized path 
// Convert oath 1m grid cell coords’ to. world coonmds 
void generate world path(path grid_path, // Path in nav grid 
path &world path); // Path in world coords 

j) Tnieral zesparn 
void path init (path &p); // Path 
// Bdd point to path 
void path_add(path &p, // Path 

int 6) mt. 9) > // Point to add to path 


// Check to see whether region around point is free of known 


obstacles 


int check clear(int x, int y); 


// Check to see whether region around point overlaps frontier 
imt check frontier arrival (int x, ant yy; cimtetrone si moea, 


// Finds waypoint furthest on path within destination tolerance, or 
// waypoint on path <p> closest to (x, y), returning the distance 
// to that point, and the waypoint's index in <index> 


inch) 


double closest waypoint (path p, // Path 
ite ee ee // Current position (17 m0 
int index, // Index of current waypoint 
int &close_index) ; // Index of closest waypoint 


LER EKER KEKKEKKKEKKKEKEKK CORRIDOR FUNCTIONS ek eek oe ek keke ke ok te ok ok ke ee de oe / 


// Detect freespace cooridors in vicinity of robot 
void detect corridors (void) ; 


// Check whether a corridor exists centered around sensor <center> 
// Bezurn 1Wat true, U-otherwise 


iMbuehneck Corrmrdor(int céneer, // Index of sensor in center of 
Corridor 
int fwd_range, // Required forward space 
int side clear); // Required side space 
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1039 
1040 
1041 
1042 
1043 
1044 
1045 
1046 
1047 
1048 
1049 
1050 
1051 
1052 
1053 
1054 
1055 
1056 
1057 
1058 
1059 
1060 
1061 
1062 
1063 
1064 
1065 
1066 
1067 
1068 
1069 
1070 
107] 
1072 
1073 
1074 
1075 
1076 
1077 
1078 
1079 
1080 
1081 
1082 
1083 
1084 
1085 
1086 
1087 
1088 
1089 
1090 
1091 
1092 
1093 
1094 
1095 
1096 


7// Check whether <sensore is clear for Ccooridor <center> 


int corridor check _sensor(int center, // Center sensor index 
int sensor, // Sensor index 
int fwd range, // Required fwd space 
int side clear); // Required side space 


// Display corridors in robot window 
voud display corridors |vo1d)? 


// Display corridor boundaries centered around sensor <center> 
void display corridor (window *win, // Window 
int center, // Center sensor index 
int fwd_range, // Required forward space 
int side clear, // Required side space 
chaz ~colonr)- jf “€Ocrs0dor color 


// Display corridor boundaries centered around sensor <center> in 


robot window 


void display corridor robot(int center, // Center sensor index 


int fwd range, // Required forward space 
int side clear, // Required side space 
nt, Color) 2. 7/7 “Corridor color 


// Select corridor nearest to specified heading 
int select corridor(double heading) ; // Heading (degrees) 


/* +x +4 INTERFACE TO CONTINUOUS LOCALIZATION ****#**### / 


// Unitialize communications with continuous localization 
VOlGdsconnecr acl (vold), 


77 send’ global, grid to-continucus Localization 
void send _ cl grid(void) ; 


/* + kK MULTIROBOT COMMUNICATION **##*#**#* / 


/7/ pina tvalaze robot Conmunication channe! 
ved Init arebot comm (void) > 


// Send message to other robot 
void send _robot_message(char *message) ; 


// Send message to other robot (user mode) 
void user send robot _message(void) ; 


// BEGIN SCOUT THESIS CHANGE 

// Receive message from other robot 

// Returns 1 if message received, 0 otherwise 

int receive robot_message(int channel, char *message) ; 


//-2ND- SCOUL THESIS, CHANGE 


// Receive message from other robot (user mode) 
void user receive robot _message(void); 


/******4*4% MULTIROBOT EXPLORATION ***#*# xe #x / 
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1097 // Integrate new map from remote robot (if a new map exists) 
1098 void integrate remote_map(void); 

HOO? ys 

1100 

1101 #endif 


22 


WO YAN BW 
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APPENDIX H. FRONTIER-BASED EXPLORATION CODE — AGENT.CC 


This appendix contains the source code for the routine that controls the robot’s 


exploration behaviors. 
/* 
agent.cc 


Agent class 
Original code by Brian Yamauchi 


Modifiicakitons for ScOUr THESIS 
By Patrick A. Hillmeyer 


ey 

#include <iostream.h> 
#include <math.h> 
#include <string.h> 
#include "agent.h" 


// Arc direction strings 


Const Char dir Str 16j[ 20) = 


Se rorwanad: 7. fwo>rwa- le, “ewelSin | 
WbebEts: “ibaekslioie, “back-LE. 


"pack. “back=baeck-re" > 7 Dack-re’, 


"right", "fwd-rt-rt", "fwd-rt", 


Consetchar voice ster Le) fsietans— 


ie roOrWworad Oy 5." fORWarad less SoOrwargd lert-2. , 
Wilett 43") “bettso."} “back eta o. 
MBaCKoO sr 4. “DaeGk. oo, “back rons O09 | right <he 
Pr lont 2. “snLgnt, bs.) 2 forward right, bd") 7 forward 5.5 7 


fa eo ee AGEN soba CONSTRUCTOR: 2 orn Say 


agent: :agent (void) 


{ 


char Global Grid{ STRLEN] ; 
char Control Panell[ STRLEN] ; 


V7 TP CCASLEUCE OL 


char labels[ MAX CON][ CON_LEN] ; 
area 


// Initialize mode flags 


multi mode = 0; 
behavior mode = EXPLORE MODE; 
contloc_mode = 0; 


home terse a a0, 
destin = 0; 


7//Glebal=Grad label 
77 Gontrol. Panel Label 
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wEwawlas 
“DaGeaback-1n > 
“baeck=bEebt g 
"fwd-fwd-rt" 


wilest. Se", 
WeAciagne’s 


108 
109 
110 


// Initialize graphics flags 


global retresh = 4; 
realtime display = 1; 
// Initialize frontier counters 


HUMEEEOnNt = .07 
num anee = v- 


// Initialize power variables 


cpu volt = CPU_FULL_VOLTAGE; 
motor volt = MOTOR FULL VOLTAGE; 


cpu min = cpu_volt; 
MOCO Minv~= mouor VOlL; 


// Initialize abitrator windows 


speed arb = new arbitrator (SPEED RES, SPEED MIN, SPERDEMss, 
SPEED eDEY,. 10; 
SPEED NOISE) ; 
if (speed arb ==" NUE) 4 
cout << "agent::agent: Unable to allocate space for speed 
arbit rarer.” 
<< endl; 
exit (1); 


} 


turn arb = new arbitrator(TURN RES, TURN MIN, TURN MAX, TURN DEE 
TURN NOISE); 
1f (turn arb == NULL) 4 
cout << "agent::agent: Unable to allocate space for turn 
anOUEratCOr.’ 


<< endl; 
exit (-1); 

} 

ie speed window.init(SPWIN X, SPWIN_Y, SPWIN_WIDTH, SPWIN HEIGHT, 
"Speed", 

le SPEED RES, SPWIN MIN, SPWIN MAX); 

vy turn _window.init(TUWIN xX, TUWIN Y, TUWIN WIDTH, TUWIN_ HEIGHT, 
eigen 

v7 TURN RES, TUWIN MIN, TUWIN MAX); 


// Initialize control window 


strcpy (labels[ CMD EXPLORE], "EXPLORE" ); 

strepy (labels[ CMD NAV], “NAVIGATE” ); 
strcpy(labels[ CMD _NAV_KBD], "NAVIGATE (KBD)"); 
Sstvcpy(labelsl CMD STOP), “SlOr.); 
strcpy(labels[ CMD SAVE], "SAVE APN"); 
strcpy(labels[ CMD LOAD], "LOAD APN"); 

strcpy (labels[ CMD REDRAW] , "DISPLAY APN"); 
strepy (labels[-CME: BUILD GRID); "BUILD GRID’); 
strcpy(labels[ CMD SAVE GRID], "SAVE GRID"); 
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142 
143 
144 
145 
146 
147 
148 
149 
150 
151 
2 
53 
154 
55 
156 
I / 
158 
IES 9 
160 
161 
162 
163 
164 
165 
166 
167 
168 


strcpy(labels[ CMD LOAD GRID], "LOAD GRID"); 
strcpy (labels[ CMD_GRID] , "DISPLAY GRID"); 
sttepy(labels| CMD CLEAR] ; “CLEAR GRID"); 

strcpy (labels[ CMD CLEAR ROBOT], "CLEAR ROBOT (ABS)"); 
strcpy (labels[ CMD_SONAR_ SCAN], "SONAR SCAN" ) ; 

strcpy (labels[ CMD_ SONAR | _SWEEP] , "SONAR SWEEP" ) ; 

Strcpy (labels[ CMD_SONAR_SWEEP ABS], "SONAR SWEEP (ABS)"); 
strcpy (labels[ CMD_CLEAR_SONAR], "CLEAR + SONAR SWEEP" ) ; 
strcpy(labels[ CMD LASER SCAN], "LASER SCAN"); 

strcepy (labels[ CMD LASER SWEEP], "LASER SWEEP") ; 
strcpy(labels[ CMD LASER SWEEP ABS], "LASER SWEEP (ABS)") ; 
stercpy (Labels| CMD) CLEAR > _ LASER) , "CLEAR + LASER SWEEP") ; 
strepy (labels[ CMD_LLS _SCAN] , “LLS SCAN" )> 

Strcpy (Labels| CMD<LLS SWEEP) {. “LLUS SWEEP); 
strcpy(labels[ CMD LLS SWEEP ABS], "LLS SWEEP (ABS)"); 
strcpy (labels[ CMD CLEAR _LLS] , MELEAR. + LLS SWEEP"); 
strcpy(labels[ CMD GRID UNDO], "UNDO SCAN/SWEEP") ; 

strcpy (labels[ CMD GRID IDENT], “GRID IDENT"); 


strcpy (Labels| CMD CENTER] , "PLACE CENTER") ; 
strcpy(labels[ CMD PLACE MAP], "PLACE MAP"); 


Serepy (labels) CMP PELACKE (GRD) Daria Y PuACr. Galkb 
strcpy (labels[ CMD LOCAL NAV], "LOCAL NAVIGATION" ) ; 
strcpy (labels[ CMD . ADD PLACE], “ADD PLACE”); 

Strcepy (labets[ CM» EDIT TELEACE| (2 EDET PEACE”); 
strcpy(labels[ CMD ADD EDIT LINK], VADD/EDIT. LINK); 
Sserepy (Labels| CME RUE EETE LINK 2 Pabses LINK’); 

strcpy (labels[ CMD CLEAR GLOBAL], "CLEAR GLOBAL GRID"); 
strcpy (labels[ CMD_SAVE GLOBAL], "SAVE GLOBAL GRID"); 
strcpy (labels[ CMD LOAD GLOBAL], "LOAD GLOBAL GRID"); 


[ 
[ is = 
strcpy (labels[ CMD PLACE IDENT], "PLACE IDENT"); 
[ 
[ 


strcpy (labels[ CMD DISPLAY GLOBAL] , "DISPLAY GLOBAL GRID"); 
strcpy (labels[ CMD GLOBAL UNDO], "UNDO GLOBAL CHANGES") ; 
strcpy (labels{ CMD INTEGRATE GRID] , "INTEGRATE LOCAL GRID") ; 
strcpy (labels{ CMD FIND FRONTIERS], "FIND FRONTIERS") ; 
strcpy (labels[ CMD DISPLAY EDGES] , "DISPLAY EDGES") ; 

strcpy (labels[ CMD DISPLAY FRONTIERS] , "DISPLAY FRONTIERS") ; 


strcepy(labels[ CMD GOTO FRONTIER], "GO TO FRONTIER"); 
strcepy (labels| CMD UPDATE NAV GRID], “UPDATE NAV GRID"); 
Str coy (labets|) CMP sDETECE _CORRIDORS] , "DETECT CORRIDORS ); 
strcpy (labels[ CMD _ CONNECT CL), "CONNECT TO CONTEOG >. 
Sercpy. bade kel eMpe SEND CL GRID), "SEND CONTLOC GREED 
strcepy(labels{[ CMD BUMP], "BUMPER TEST"); 

Sstrepy (Labels| CMD INIT COMM], “INIT ROBOT COMM’); 

strcopy (labels[ CMD SEND MSG], "SEND MESSAGE" ) ; 

strepy (labels[ CMD RECEIVE MSG], "RECEIVE MESSAGE" ) ; 


Stccpoy (Labels CMDCEAIT iy Exit); 


//BEGIN SCOUT THESIS CHANGE 


Somime &(Conerol baneiy-“ Contre LAG a Pane | yt mac) 


control window.init panel (CON WIN LEFT, CON WIN TOP, 


CON BUTTON WIDTH, 


CONF BUTTON HEIGHT, ~Gonerel Panel, 
CON _LAB _WIDTH, CON _ LAB _HEIGHT, CON _ NUM CMD, 
CON ECOLS; CON _ ROWS, labels); 

control window.draw(); 
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169 
170 
171 
2 
173 
174 
175 
176 
177 
178 
179 
180 
18] 
182 
183 
184 
185 
186 
187 
188 
189 
190 
Po) 
192 
122 
194 
195 
196 
17: 
198 
199 
200 
201 
202 
203 
204 
205 
206 
207 
208 
209 
210 
20 
22 
23 
214 
Zi 
216 
Zl 
218 
219 
220 
221 
mee 
a7 
224 
225 
226 


// Initialize evidence grid window 


grid window = new window(EGWIN_ LEFT, EGWIN_ TOP, EGWIN RIGHT, 


EGWIN BOTTOM, 


"Evidence Grid"); 
grid _window->set_window(EGWIN _WC_LEFT, EGWIN_WC_BOTTOM, 


EGWIN WC_RIGHT, 


EGWIN WC TOP); 
grid window->iconify(); 


// Initialize navigation grid window 


Jy nav_ window = new window(NAV_WIN_ LEFT, NAV_WIN TOP, 
NAV WIN RIGHT, 
wy. NAV WIN BOTTOM, "Navigation Grid"); 
ee nav_window->set_window(NAV_WIN_ WC_LEFT, NAV_WIN_WC_BOTTOM, 
NAV WIN WC_RIGHT, 
—// NAV WIN WC TOP); 
// nav_window->iconify(); 


// Initialize global evidence grid window 


Sprint? (Global Grid, Globo iecd "Grid airo1d). 


global window = new window(GLOBAL WIN LEFT, GLOBAL WIN TOP, 


GLOBAL _ WIN _ RIGHT, GLOBAL _ WIN _ BOTTOM, 
Global Grid); 


// END SCOUT THESIS CHANGE 


Ty, 


global _window->set_window (GLOBAL WIN _WC_LEFT, GLOBAL WIN WC_BOTTOM, 
GLOBAL WIN WC RIGHT, GLOBAL WIN _ WC TOP); 

i global _window->iconify(); 

// Initialize evidence grid sensor models 


cout << "Evidence grid: <disabled>" << endl; 


table init); 
model init(&sonar_smd, &sonar_clear_smd) ; 


// Initialize evidence grids 


Grid init (eegqrid, 020; 0.0); 
Gbid Ante(é&ola grid, 0.9, 0-0); 


grid init nav(énav_grid, 0.0, 0.0); 
Grid init .gqlobal (églebal grid, 0.0, 0.0); 
Grid init glebal (colalglobal,0a0) 10.0); 
grid init global tedge grid, 0-0, 0,0); 
// Initialize moving obstacles 
fOr (1 = 0; 1 <= NUMIMCe its) { 

mob Jist[ 1) .ramdsinve(); 


} 


// Reset timers 
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227 
228 
22.9 
230 
ZS | 
232 
233 
234 
3) 
236 
eS / 
238 
239 
240 
241 
242 
243 
244 
245 
246 
247 
248 
249 


251 
252 
253 
254 
255 
256 
Eo] 
258 
259 
260 
261 


263 
264 
265 
266 
267 
268 
269 
270 
271 
B12 
2/3 
274 
2/5 
276 
7 
278 
219 
280 
281 
282 
283 
284 


timer = QO; 
// Initialize file pointers 
logfile = NULL; 
// Turn on all sensors 
TL, Sonar On); 
Eien (), 
Beaser on, |); 
// Initialize cell count 
ecl counts — 0; 
// BUMPER FIX INITIALIZATION 
POuw GeO; 1°< NUM TOUCH, att) 4 
bumped{ i] = 0; 
} 
} 
[RR KKK KK KKK USER CONTROL FUNCTIONS KEKKKKKKEK / 
wera agent: :>control (void) 
// Main control loop 
inte quit — 0; 
adicr{ 


guit = user command(); 


} 
while (!quit); 


Wierd agent: : power check{void} 


{ 
// Check battery power 


Char VYoOstr) STREEN] > (I PIOUCE? String 

gs(); 

cpu_ volt = (double) (int) (voltCpuGet() * 100.0) / 100.0; 
MNCEOravolte-— ‘(deuple) (int) (vel tMovorGer(j,* “100.07 100 sa: 

77 COut <<") Cru voltage = = <<"cpu volts< "2 metros tog 


motor volt 
ay <—eenai, 


j/7/ “Court << "CFU voltage =" << -voltCpuGet() <<" = motor sveltaqe. — 
Th << voltMotorGet() << endl; 


aE Cpu ole se pur mint 
CpUTHAIn =sepulvelec; 


ZG 


285 
286 
287 
288 
289 
290 
29) 
292 
293 
294 
295 
296 
Zo] 
298 
e° 
300 
301 
302 
303 
304 
305 
306 
307 
308 
309 
310 
31] 
B12 
a3 
314 
o> 
316 
St 
318 
Bo 
320 
oz | 
B22 
323 
324 
325 
326 
oe] 
328 
B29 
330 
oS | 
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334 
335 
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O37 
338 
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340 
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342 


if (cpu_volt < CPU DANGER VOLTAGE) { 

Sprintt(vostr;, “Danger, Danger: C P Ulvoltage 14 %.2.- ns 
cpu_volt); 

COuUt == Voor. 
Ck lvOSEr)s; 

} 

else if (cpu_volt < CPU_FULL VOLTAGE) { 
Sprinti(vostr, "Warming: € FP U voltage 2s 5%.20-\n 7 scousvoliur 
COUE <= VOoEr- 
Ek (VoOstr); 

} 

} 


TE o(MOL Or VOl~ = moron sat) 

MOCO MIN — MobLor volt; 

if (motor _volt < MOTOR DANGER VOLTAGE) { 
Sprintit(vostzr, “Danger, Danger: Metor voltage is $.2Zf. in | 

motor volt); 

COUE <= VOSEL: 
EkK(VOstxr) 7 

} 

else if (motor volt < MOTOR FULL VOLTAGE) { 
Sprintiivostr, “Warning: "Motor voltage 15 °%. 712 \n'; motor voli 
COuE ==eVOSeEr: 
EM(Voct.y? 


} 


int agent::user command (void) 


{ 


// Execute user command (if any) 


iMt quite 0; // Set to 1 for exit command 
int command; // Command code 


// power check(); 


control window.refresh(); 
command = control window.scan_ panel (); 


Switch(command) { 

case CMD EXPEORE: 
exploration lis(); 
break; 

case CMD NAV: 
navigation (); 
break; 

case CMD NAV KBD: 
navigation_keyboard(); 
break; 

case CMD SAVE: 
save net(); 
break; 

case CMD LOAD: 
load _net(); 
break; 

case CMD REDRAW: 
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343 
344 
345 
346 
347 
348 
349 
350 
BDL 
552 
B53 
354 
355 
356 
357 
358 
Boo 
360 
361 
362 
363 
364 
365 
366 
367 
368 
369 
370 
37] 
By 2 
B73 
374 
B/D 
376 
B77 
378 
B19 
380 
38] 
382 
383 
384 
385 
386 
387 
388 
389 
390 
39] 
52 
B93 
394 
B95 
396 
B97 
398 
Bo? 
400 


// 


// 


pnet.display(); 
break; 

case CMD BUILD GRID: 
r.update(); 
grid clear (egrid); 
clear robot(egrid, 0, 0); 
sonar sweep seq(egrid); 

laser sweep seq(egrid); 


grid display(grid_window, egrid); 


break; 

case CMD SAVE GRID: 
Save grid(egrid); 
break; 

case CMD COAD GRID: 
foad. grid (éegqrid), 
r.update(); 


grid display(grid_ window, egrid); 


break; 
case CMD GRID: 
r.update(); 


grid display(grid_window, egrid); 


break; 

case CMD CLEAR: 
Gridsecpy (eld lonid, “eqn1d), 
grid clear(egrid); 


grid display(grid_window, egrid); 


break; 
case CMD CLEAR ROBOT: 
grad copy (ola grid, eqrrd); 
r.update(); 
lear Obeblegrid; 2k, 1. Vy) 


grid display(grid_window, egrid); 


break; 

case CMD SONAR SCAN: 
Geld Copy (old grid, egrid); 
Boupcate ()¢ 


SCOUT THESIS CHANGE - in line below changed r.turret to r.theta 


Sonar scCan(sonar smd, sonar clear smd, 
grid display(grid_window, egrid); 


break; 

case CMD SONAR SWEEP: 
Guia copy (old dre1d, egrid); 
elear robor (equi, 0, 0); 
Sonar | Sweepsseq (egrid); 


grid display(grid_ window, egrid); 


break; 

case CMD SONAR _SWEEP ABS: 
grid copy(old_ grid, egrid); 
r.update(); 
Cli@areEOoOrRiegmad. ax, tr. Vy) 
sonar sweep abs seq(egrid) ; 


grid display(grid window, egrid); 


break; 

case CMD CLEAR SONAR: 
Grideecony oldggrid  egrid) | 
Grlatelean(egqrid); 
Yr .update ()? 
Cleat robot (eqrid, » 0,0), 
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401 
402 
403 
404 
405 
406 
407 
408 
409 
410 
411 
412 
413 
414 
415 
416 
417 
418 
419 
420 
421 
422 
423 
424 
425 
426 
427 
428 
429 
430 
431 
432 
433 
434 
435 
436 
437 
438 
439 
440 
44] 
442 
443 
444 
445 
446 
447 
448 
449 
450 
451 
452 
453 
454 
455 
456 
457 
458 


sonar sweep seq(egrid); 
grid display(grid_ window, egrid); 
break; 
case CMD LASER SCAN: 
Grid copy (old ignid,\egrid) ; 


r.update(); 
// Replaced r.turret with r.theta in line below 
laser sCan(egrid, ©-x, r.y, r.theta); // SCOUT THESts Chandcse. 


Scout with fixed body laser 

grid display (grid_window, egrid); 
break; 

case CMD LASER SWEEP: 
ria copy (oldsguid, egrid); 
r.update(); 
Lasers sweep sacQvegaid).; 
grid_display(grid_window, egrid); 
break; 

case CMD LASER SWEEP ABS: 
Grigdseooy (old grid, “eqrid) ; 
r.update(); 
laseuesweep abs seq (eqrid); 
grid display(grid_window, egrid); 
break; 

case CMD CLEAR LASER: 
grid _copy(old_grid, egrid); 
grid_clear(egrid); 
r.update(); 
laser sweep seq(egrid); 
grid _display(grid_window, egrid); 
break; 

case CMD LLS_SCAN: 
grid copy(old grid, egrid); 
r.update(); 
lls _scan(sonar_smd, sonar_clear_smd, egqrid, ©.k, 2,7. enero 

SCOUT THESIS - see change above 

grid_display(grid_window, egrid); 
break; 

case. CMD LILS SWEEP: 
grid _copy(old grid, egrid); 
r.update(); 
clear robot(egrid, 0, 0); 
lls _sweep abs seq(egrid); 
grid display (grid_window, egrid); 
break; 

case CMD LLS SWEEP ABS: 
grid _copy(old_ global, global grid); 
r.update(); 
clear robot (global grid, 0, 0); 
lis sweep seq(global grid); 
grid dasplay global (global grid}; 
break; 

case CMD CLEAR LLS: 
Jriaucepy(olargrid, eqn); 
Grideclearnveqrud); 
r.update(); 
Cléeamz robot (equid, 0; 0); 
lls sweep seq(egrid); 
grid_display(grid window, egrid); 
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break; 

case CMD GRID UNDO: 
grLa Copy (egrid, ofd grid); 
grid _display(grid_ window, egrid); 
break; 

case CMD GRID IDENT: 
gnigyrdent seq) 
break; 

case CMD CENTER: 
center seq({); 
break; 

case CMD PLACE MAP: 
map seq(); 
break; 

case CUDT PEACE TRENT: 
ident _seq({); 
break; 

case CMD PEACE GRED: 
display place grid(); 
break; 

case CMD LOCAL NAV: 
local navigation (); 
break; 

case CMD ADD PLACE: 
pnet.aad place); 
break; 

case CMD EDIT (PEACE. 
pnet.edit place(); 
break; 

ease CMD ADE EDIT LINK: 
pnet.add_ edit link(); 
break; 

case CMD DELETE LINK: 
pnet.delete link(); 
break; 

Case CMD CLEAR GLOBAL: 
grid copy (old gicbal, global grid); 
grid clear(qieobal grid); 
grid display global(global grid); 
num front = 0; 


num visit = 0; 
num inac = 0; 
break; 


case CMD SAVE GLOBAL: 
save _gridi(global grid); 
break; 

case CMD LOAD GLOBAL: 
iead grid(éqlcbal grid); 
r.update(); 
Geldedusplay iglobal{ Global grid); 
break; 

case CMD DISPLAY GLOBAL: 
gridy display global (global grad); 
break; 

case CMD GLOBAL UNDO: 
GrUamcOpy (Globe lscrrd sola rotenal) > 
grid_display global(global grid); 
break; 
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ease CMD INTEGRATE GRIP: 


integrate grid(global grid, egrid, (double) r.x / 120.0; 


(double) r.y / 120.0, (double) 
grid Gisplay global (global grid); 
break; 

Case CMD EEND SS RONTIERS: 
Pine. frene1ens |); 
break; 
case CMD DISPLAY EDGES: 
Grudidisplay global (global vogrid); 
grid display edges(region_map); 
break; 
case CMD DISPLAY FRONTIERS: 
grid display global(global_ grid); 
grid display regions(region_map) ; 
display region _centroids(0.0, 0.0); 
// display robot region centroids (); 
break; 
case CMD GOTO FRONTIER: 
frontier navigate(); 
break; 
case CMD UPDATE NAV GRID: 
update nav_grid(); 
break; 
case CMD DETECT CORRIDORS: 
detect corridors(); 
display corridors (); 
break; 
case CMD CONNECT CL: 
connect Cl (); 
break; 
case CMD SEND CL GRID: 
Sendscl grid) 
break; 
case “CMD BUMP: 
bump test(); 
break; 
case CMD INIT COMM: 
init _robot_comm(); 
break; 
case CMD SEND MSG: 
user send _robot_message(); 
break; 
case CMD RECEIVE MSG: 
user receive robot_message(); 
break; 
Case CMDMEA ET: 
terminate(); 
quit = l; 
break; 
} 
return (quit); 


} 


void agent: :terminate(void) 
{ 


j// Ena session 
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Inc. 1: 
// Delete mobstacles 


for 42 = 07 i NUM MOR. a++) { 
MOD list) a) .delvebs(); 
} 


// Shut down robot 


reshurdown() = 


} 


int agent::iscan(void) 
{ 


// Scan for interrupt 
int command; 


Contre. Window. er resi) 7 

command = control window.scan panel(); 

if ((command == CMD STOP) || (command == CMD EXIT)) { 
st(); 
return (ABORT) ; 

} 

evse 4 
SCcurniOn) ; 

} 

} 


/********** BEHAVIOR CONTROL SYSTEMS *****# 4% ex / 


void agent::bump test (void) 
{ 
Grad display Global (qlobalsgrid); 


while(iscan() != ABORT) { 
update(); 
bump _halt(); 
} 
} 


VOLG-agent..manualee<plLoration( void) 


{ 


// Map territory under manual control 
int net status; // Place net changed status 


tamer = 0; 
behavior mode = EXPLORE MODE; 


// BEGIN SCOUT THESIS CHANGE 

scout _vm(0, 0); // Necessary hack so robot will start moving later 
//SCOUL. THESIS CHANGE — changed prsto vm 
// END SCOUT THESIS CHANGE 


manual exploration _seq(); 


} 
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void agent: :exploration (void) 

{ 
// Explore territory 
behavior mode = EXPLORE MODE; 


exploration seq(); 


} 

void agent::exploration lls (void) 

: // Explore territory using laser-limited sonar 
char comm_str[{ STRLEN] ; // Contloc communication string 
behavior mode = EXPLORE LLS MODE; 
// Set relocalization interval 
Sprintft(comm_ str, "“reloc distance = ¢d", EXPLORE RELOC DISTANCE 
Ccouc << “comm str = )<"9 )<- commestr <<) > )=- char, 
write comm(COMM CHANNEL, comm_str, strlen(comm_str) + 1); 


// Exploration sequence 


exploration lls seq(); 


j 
void agent::reactive_ exploration (void) 
{ 


// Explore territory reactively 


int net status; // Place net changed status 


// char logname[{ STRLEN] ; // Log file name 
// char apnname[{ STRLEN] ; // APN file name 
timer = QO; 


behavior mode = EXPLORE MODE; 


(odor 
cout << "Enter log file name ==> "; 
Cin >> logname; 


logfile = new ofstream(logname) ; 
if (logfile == NULL) { 
cout << "Unable to open log file <" << logname << ">." << ena 
} 
} 
while (logfile == NULL); 


cout << "Enter APN file name ==> "; 
cin >> apnname;* / 


// xeset(); 
// pnet.clear net(); 


update (); 
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// 


net status = pnet.place learn((double) r.x, (double) r.y, 
7 (double) r.theta / 10.0); 

it Hmee Status ¢ NEW PLACE) { 
map seq(); 

} 


reactive exploration_seq(); 


logfile->close(); 


// pnet.save(apnname) ; 


} 


void agent: :multi_ exploration (void) 


{ 


// Explore territory (multiple trials) 


char prefix[ STRLEN]); // Filename prefixes 

char logname[ STRLEN] ; // Log filename 

char apnname{ STRLEN] ; // APN filename 

int trial index; // Trial index 

itotpial sstart, // Index for initial trial 

Pie trial end; ~7 Index for sas eridgl 

int rand_x, rand_y, rand_heading; // Random initial position 


behavior mode = EXPLORE MODE; 


cout << "Enter filename prefix ==> "; 
Cine pletlx, 


cout << "Enter starting trial number ==> "; 
ein 22 trial starG; 


cout << "Enter ending trial number ==> "; 
Clim 22 (tlie lend,; 


ber (erie ndet = Erialustant, trial index <= Lrialvend, 


trial index+t+) { 


RAND MARGIN) ; 


sprintf (logname, "S500.109", pretix, trial. index)? 
Sprincs’ (apnname, “tsta.apm , pretax, trial andex); 


logfile = new ofstream(logname) ; 
if (logfile == NULL) { 
cout =< “Unable to open log file <" << llogname << ">." << endl; 


} 
else { 
e0ut << "Opening Jog .file=<" << logqname << ">." <<énd.-; 


} 


ZeSet(): 
Pmer. clear ner), 


// Set random initial position 
irand(PWIN WC LEFT + RAND MARGIN, PWIN WC RIGHT - 


rand_y = irand(PWIN_WC_BOTTOM + RAND MARGIN, PWIN WC TOP - 


RAND MARGIN) ; 


rand heading = irand(0, 3600); 
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place robot(rand_x, rand_y, rand_heading, rand_heading) ; 
// Hack to make sure robot isn't teleported into wall 


// BEGIN SCOUT THESIS CHANGE 
scour. viiliin <0) 7 // TEMP FIX- changed pr to vm 
Scout Vint i4 80); // TEMP FIX - changed pr to vm 
// END SCOUT THESIS CHANGE 


update (); 
pnet.place Vearn( (double); rox, (double; ©.y, (double) = stheeag. 
EO Oa; 


exp loraeion seq), 


if (logfile != NULL) { 
logfile->close(); 
} 


pnet.save(apnname) ; 


} 


void agent: :multi_reactive_ exploration (void) 


{ 


// Explore territory reactively (multiple trials) 


char prefix[ STRLEN]); // Filename prefixes 

char logname[ STRLEN] ; // Log filename 

char apnname[ STRLEN] ; // APN filename 

int trial index; // Trial index 

Int celal jstare; j/ imdex ter initial trial 
Int trial ne, // Index for Vast trial 


behavior mode = EXPLORE MODE; 


cout << "Enter filename prefix ==> "; 
Cit. 2S Dretix> 


cout <<" Enter Starcing trial number. ——- 
Cilmi 72 Gelal stare, 


cout << "Enter ending trial number ==> "; 
Clie (brie lena, 


for(triai index — "trial stare, Erial index <> trial end: 
Cela ingex +t) | 
sprintf(logname, "%s%éd.log", prefix, trial index); 
sprintf(apnname, "%std.apn", prefix, trial index); 


logfile = new ofstream(logname) ; 
if (logfile == NULL) { 
cout << "Unable to open log file <" << logname << ">." << endae 
} 
else { 
cout << "Opening log file <" << logname << ">." << endl; 
} 


reset(); 
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807 pnet.clear net(); 


808 
809 update (); 
810 pnet.place learn((double) r.x, (double) r.y, (double) r.theta / 
Sil = s10..0); 
812 
813 reactive exploration_seq(); 
814 
815 Lf Wlogrile (= NUL). 4 
816 logfile->close(); 
817 } 
818 pnet.save(apnname) ; 
819 } 
820} 
821 
822 void agent: :navigation (void) 
BS CO{ 
824 // Navigate to destination specified with mouse 
825 
826 char comm_str{ STRLEN] ; // Contloc communication string 
827 char vostr{ STRLEN ; // Voice string 
6 double gx, gy; // Destination point (world coords) 
a // Wait for user to click on destination in global window 
] 
a grid display global(global_ grid); 
es while(global_ window->world_ mouse(gx, gy) == 0); 
836 sprintf (vostr, “Navigating to $d, Sd.\n" pint) sox (iat) oy 
837 Ceouks<< VOSLE;, 
838 ek(vostr); 
839 
ye // Mark destination in window 
842 global _window->set_color ("red"); 
843 global_window->display circle(gx, gy, CENTROID MARK RADIUS) ; 
844 global _window->display line(gx - CENTROID MARK RADIUS, gy, 
845 gx + CENTROID MARK RADIUS, gy); 
846 global window->display line(gx, gy - CENTROID MARK RADIUS, 
847 i gx, gy + CENTROID MARK RADIUS); 
848 global window->set color("black" ); i - 
849 7 7. 
oa // Set relocalization interval 
852 sprintf(comm str, "“reloc distance = $d", NAV RELOC DISTANCE); 
$53 cout << "comm str = <" << comm str << ">" << endl; _ 
Bos write comm(COMM CHANNEL, comm str, SED len (Gommsst se). 7 
856 // Navigate to destination 
857 
858 refresh all(); 
859 path nav seq(gx, gy); 
860 = 
861 PemMever tos) W(ime ox, ane gy) 
862 r.face angle(0)-; 
863 7 
864 Peele, // Sometimes garbage gets stuck in the voice buffer 
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} 


sprintf(vostr, "Arrived at destination.\n"); 
COUE << VOSEL; 
tkhtivoster)- 


void agent: :navigation_keyboard(void) 


{ 


// Navigate to destination specified with keyboard 


char comm_str[ STRLEN] ; // Contloc communication strang 
char vostr{ STRLEN] ; // Voice string 

double gx, gy; // Destination point (world coords) 

double gtheta; // Destination orientation 


jj BS User to enter destination 


cout << “Enter destination (x, y, theta) (1/10 in, 1/10 deg) ==-3% 
Cin SS G4 o> oy >> Ggtheta-: 


sprintf (vostr, "Navigating to ¢d, $d (%d).\n", (int) ox tints 
(int) gtheta); 
COouL << VOStr: 


Ekivestxr)> 
// Mark destination in window 
Grad display sglobals(qlobal grid); 


Global wandow=-Seu Ccolor(™ red |; 

global _window->display circle(gx, gy, CENTROID MARK RADIUS) ; 

global ~ window- =-disolay 7 ine( gx = CENTRO ED _ MARK _ RADIUS, gy, 
gx + CENTROID MARK _ RADIUS, Gy)? 

global window->display line(gx, gy - CENTROID MARK RADIUS, 
gx, gy + CENTROID MARK RADIUS) ; 

global wingow->ser Color black); 


// Set relocalization interval 

sprintf£(comm_ str, "reloc distance = %d", NAV RELOC DISTANCE); 
cout << "Comm str i= << comm stu << "2S" <<sendl, 

write comm (COMM -CHANNEL, comm str, strlen (comm str) =: -1) 7 


// Navigate to destination 


Serres allt), 
path nav_seq(gx, gy); 


r.MOveintowys (me) qa ane) yi, 
r, face vangle(jant) guneta), 


Che ie // Sometimes garbage gets stuck in the voice buffer 
sprintf(vostr, "Arrived at destination.\n"); 


Cour << YVOSEE; 
Eka VOsStL) 
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void agent::local navigation (void) 


{ 


// Navigate to local coordinate point 
ine, x; Vy; // Local destination coordinates 


cout =< “Enter destination point (x, vy) ==> "; 
Cin So 0% 2 oy; 


local nav_seq(x, y); 


} 


wold agent::frontier navigate(void) 


{ 


// Navigate to frontier centroid 


Hit Lrone. 1andex; // MiNGe x Or CESEANae On fe Oned cr 
ete (Hum One =— 30) { 
cout << "No frontiers detected." << endl; 
return; 
} 
do { 


cout =< “Enter frontier oandex ==> "> 
Cin >> front index; 


ft fl erence index = 10) (| (ivone andex-—i pum front) ) 4 
cout << “Unknown frontver =- must be an range | 0.." << numetront 
pace 7c 
<< endl; 
} 
} 
Wile ( (front index 9-0) || (ivont imecex 2 mume fronts) 


ErOneaer Nav seg (frone index); 


} 


[KKK KK KKK BEHAVIORAL SEQUENCERS Re KKK KKK KK / 
void agent::manual exploration_seq(void) 
{ 


// Manual exploration sequencer 


int net_status; // Place net changed status 


couty<<""fxplorang under manual =control...: “<< end); 
do { 
update(); 
net status™— pnee- place learn (double) xp ex7eicouple) 77, 


(double) r.theta / 10.0); 


if (net_status & NEW PLACE) { 
GOULET SEO. 41) a— endl; 
ek ("Stop . sp 
Ste. 
WS ps ieee) 
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map seq), 
} 
} 
while(iscan() != ABORT); 


cout << "Exploration complete." << endl; 


} 


VOld agent: -exploration seq (void) 


{ 


// Exploration sequencer 


int front index = 0; // Frontier destination index 
int nav Status = OK; “// ‘Navigation Status 


Gout (<< exploring... << endl; 
Gi Ex lotsa. ); 


update(); 
elear robot (global grid, ©.x, ©r.y); 
sonar sweep abs seq(global_ grid); 


Hine se bone Hers) ; 
while((num_front > 0) && (nav_status != ABORT) ¢& (front_index != Samy 
front <index — closest frontier({ (double) r-x, (deubte; nye 


a Sul conc. 1 neex “f=. =o 
Mavastdeus — —rOnEler Mav seq( fron anicex); 
tae cleat robber (Global guild, tac, bay) 
vf sonar sweep seq(global grid); 
EIMeGerEonererst):; 
} 
} 


cout << "Exploration complete." << endl; 
tk("Exploration complete."); 


} 


void agent: :exploration lis seq(void) 


{ 


// Exploration sequencer using laser-limited sonar 


char local filename [ STRLEN] ; // Filename for local grid 

char Tecal posinre [SUREEN|; // Position info for local ¥aimag 
file 

char global filename[ STRLEN] ; // Filename for global grid 

char global posinfof STRLEN] ; // Position info for global grr 
file 

char message[ STRLEN] ; // Message for multirobot communications 

double tx = 0.0, ty = 0.0; // Registration translation vector 

double ttheta = 0.0; // Registration rotation 

double score; // Registration score for local 


Grid 
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int front index = 0; // Frontier destination index 

int nav_status = OK; // Navigation status 

I NELOGe; // Namber of occupied cells in global grid 

it unece; // Number of unoccupied cells in global grid 
Gout <<. Exploring..." << endl; 


Bee Epon ung. 


// NEW SCOUT THESIS CHANGE below 

// If robot is robot 1 it is the SERVER robot and will send its global 
map out to 

// the other CLIENT robots 

// if robot is not number 1 then it is a CLIENT robot and will only 
write its 

// local scan to file 

// in this way I hope to slow down error buildup in the map 


if (r.id == 1) { 
Sprantt (global filename, “Globaltd seq 7 ©.id); 
// Sweep from initial position and find frontiers 


update(); 
lear -rebou(global grid, rex, ray); 


// BEGIN SCOUT THESIS CHANGE 
// instead of using laser limited sonar use just the sonars 


// 11s_sweep_abs_ seq(global_ grid); // commented out for Scout 
sonar_sweep_abs_seq(global_grid); // use sonars only to explore 

(7 @END SCOUT THESIS CHANGE 
// grid_display(grid_window, egrid); 
// Save global grid 


Sprints (global posintie, "sd td td’, 0, 0, 0); 
Save Gr10 Tile(qlobal grid, gichal filename, global posinio),; 


// Notify other robot 
ie. Me i mode | { 
send robot_message(global_ filename) ; 
} 
// Display global grid 
Gridvdisplay global glebal grid), 
7/7 Send grid. cto continuous localization 
Guidrcountwoce (Gg lebalegrid, 7eOce,. sinece):: 
Coun << "Global vonid cells: mapped =" << 6c¢.7+-unoce 


Say OCECUDLEC « =) “Ka OCe << endl; 
Der (Oee es — CONTLOG Mil OCe )x{ 
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send cl igrid(); 


// Check for new map from other robot 
if (multi_mode) { 

integrate _remote_map(); 
} 


// Find initial frontiers 


fine rrontiers {)7 


wnile(nav status != ABORT) 
of Anum trons 2 0) >; 
// Navigate to closest frontier (index = -1 if inaccessible or 
visited) 


Front, index = closest frontier( (double) rx, (double) 1.7, 
if {front aindex>!= <1) 4 
nav_status = frontier _nav_seq(front_index) ; 
} 
} 


zt (nun trone, == 0) il (trom amdex ==> ly jan 
if (iscan() == ABORT) { // add check for interrupts from 
control panel 

nav_ status = ABORT; 

} 

else { 
cout << "No frontiers remaining, sweeping sensors..." << endl; 
tk("No frontiers, sweeping." ); 


nev .status.— NOoPRONTIERS, 
} 
} 


1f ({nav status != ABORT) && (nav status != NO PATH)) { 
clear reboe(glebal rid, rx. ty) 


// BEGIN SCOUT THESIS CHANGE 
// instead of using laser limited sonar use just the sonars 


ee lls sweep abs seq(global grid); // commented out for Scout 
sonar _sweep_abs_seq(global_ grid); 

// END S@GOUT THESIS. CHANGE 
iy. grid display(grid window, egrid) ; 
// Save global grid 


Sprintti(glebal posinio;.”"2de.0 4a, 0, 0; 2); 
save grid file(global grid, global filename, global posinie), 


// Notify other robot 
if (multi_mode) { 


send_robot_message (global filename) ; 


} 
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1156 
iS 7 
1158 
1159 
1160 
1161 
1162 
1163 
1164 
1165 
1166 
1167 
1168 
1169 
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W171 
ml 72 
1173 
1174 
75 
76 
Me 7 
1178 
He] 9 
1180 
118] 
1182 
1183 
1184 
1185 
1186 
1187 
1188 
1189 
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1191 
M92 
93 
1194 
HOS 
1196 
el 97 
1198 
1199 
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1202 
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1205 
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1207 
1208 
1209 
1210 
1211 
1212 


7/7 Display Global grad 
Ggridcdasplay Global globaligrid)., 

// Send grid to continuous localization 
grid_count_occ(global_ grid, &o0cc, &unocc); 


cout << "Global grid célls: mapoec =" << o¢c + unoce 
<< " = occupied =" << oce << endl: 


if (occ >= CONTLOC MIN OCC) { 
send cl grid), 
} 


// Check for new map from other robot 
EE (multi mode) { 
INECOEaAte —SEmOee Map) ; 


} 


j// Find new frontiers 
find trent ilexs } ; 
} 
} 


} // close for if r.id==1 


// NEW MAJOR SCOUT THESIS change 

// now handle the case of the CLIENT robots that just write their 
// local maps 

else { 7 ad = 4 


Sprints (local filename, “lecaled.eq' , %.2d); 


// Sweep from initial position and find frontiers 


update (); 

Griad Clear (egrid), // clear the old local grid prior to scanning 

elear roborlegrad, 07 0); // maxk the cells under the robot as 
unoccupied 


ji Weleae reper (global grid, ©.x, 2-y)- 


// BEGIN SCOUT THESIS CHANGE 
// instead of using laser limited sonar use just the sonars 


j7 Vilsysweepiabs seq(globealxygrid); // commented out for Scout 
// S0nar (Sweep abs seq(global grid); // use sonars only to explore in 
global eesition 

sonar sweep seq(egrid); // use sonars only to make local scan 


centered around robot 
// END SCOUT THESIS CHANGE 
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12272 
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33 
1234 
1235 
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1238 
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1243 
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1245 
1246 
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1250 
1251 
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1270 


// gxvid_display(grid_ window, egrid); 


// Register local grid with global grid - necessary when using robot 


base position 


// for scanning vice global position 


tx (double) ioe ZOO 
ty (double) r.y / 120.0; 
ttheta = 0.0; 


// Save local grid 


Splines (teed posinto; 2°64 2G, di res, 0), 
save grid file(egrid, local_filename, local_posinfo); 


/7/ Notity other robor 


Le ml ea mode) { 
send robot message(local filename) ; 


} 


Inteqrare local grid with global grid 
integrate grid(global grid, egrid, tx, ty, ttheta); 


// Display global grid 
dria display lobaliglobal grid), 


// Send grad to. continuous Jocalazation 


guid count -oce (global sgrid,. socc, &unocc) ; 
cout << “Global grid cells: ‘mapped = " << occ + unocc 
<< = (occupied =<" <<"oce << endl; 
1f (occ >= CONTLOC MIN OCC) { 
Send el gridt); 
} 


// Check for new map from other robot 
if (mulcasmode) 4 


integrate remote map(); 


} 
// Find initial frontiers 


find Trepirsers |); 


while (nav status '=/78B0ORT) 4 
1f (numerene> 074 
// Navigate to closest frontier (index = -1 if inaccessible or 
visited) 


Eronbwandex = closest trontier( (double) 1.x, (double). 
if (front andex != =) { 
nav status = frontier _nav_seq(front_index) ; 
} 
} 
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1289 
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1291 
1292 
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1298 
1299 
1300 
1301 
1302 
1303 
1304 
1305 
1306 
1307 
1308 
1309 
1310 
i 11 
1312 
1313 
1314 
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1316 
1317 
1318 
#319 
1320 
1321 
S22 
23 
1324 
1325 
1326 
i327 
1328 


cpt nun erOnc == 0) || Cerenetindex == i-1))..{ 
if (iscan() == ABORT) { // check for interrupts from 
control panel 
Wav Status = AEOR!; 
} 
else { 
cout << "No frontiers remaining, sweeping sensors..." << endl; 


ae 
// 


// 
es 


Je 


tk("No frontiers, sweeping."); 
Nav Status = NO FRONTIERS: 
} 
} 


it ((mayv status 4= APORT) 66 (havestatus t= NO PATH) { 
grid clear (egrid); 
clear robot(egrid, 0, 0); 


BEGIN SCOUT THESIS CHANGE 
instead of using laser limited sonar use just the sonars 


lls sweep _abs_seq(global_ grid); // commented out for Scout 
sonar sweep abs seq(global grid); 
sonar sweep seq(egrid); 


END SCOUT THESIS CHANGE 


To grid_display(grid_window, egrid); 


// Register local grid with global grid - necessary when using robot 


base position 


// for scanning vice global position 


cx (double) 1.x 7 120.0- 
ty (double) (r.y 7 120.0; 
ttheta = 0.0; 


// Save local grid 


wo 


Sprinter (Local posinto,” 74d td td’, rk, my, 0), 
save grid fa levegqrid, local filename, -loeal posinio), 


// Notify other robot 


tf Mole mode. 4 
send robot_message(local filename) ; 


} 


// Integrate local grid with global grid 


integrate grid(global grid, egrid, tx, ty, ttheta); 
// Display global grid 

gridydisplay global(gleobaly grid); 

// Send grid to continuous localization 


GmacdPeounemoce(globalyorid, .0Ge,7cunoce),, 


Z55 


1329 cout << "Global grid cells: mapped =" << occ + unocc 
1330 <<" : occupied =" << occ << endl; 
13 | 
1532 if (occ >= CONTLOC MIN OCC) { 
1333 send _cl_grid(); 
1334 } 
1335 
1336 // Check for new map from other robot 
1337/7 
1338 if (multi_mode) { 
1339 integrate remote map(); 
1340 } 
1341 
1342 // Find new frontiers 
1343 
1344 find frontiers(); 
1345 } 
1346 } 
1347 
1348 } // close for else r.id !=1 
1349 
1350 // END NEW MAJOR THESIS change 
1351 
[352 
1353 
1354 cout << "Exploration complete." << endl; 
1355 tk("Exploration complete."); 
1356. } 
1357 
1358 
1359 
ee void agent::reactive exploration seq(void) 
{ 
ase // Reactive exploration sequencer 
1364 Gout << “Exploring reackively..."9 << endl; 
1365 Ek Exploring: Jy 
1366 
1367 ao: { 
1368 update (); 
1369 
1370 set_defaults(); 
137 | if (reactive explore behaviors() == 0) { 
1372 execute(); 
l373 } 
1374 } 
eee while((iscan() != ABORT) && (timer <= TRIAL LENGTH) ); 
1377 cout << "Exploration complete." << endl; 
1378 3} 
1379 
1380 int agent::navigation seq(void) 
Hol { i. 
1382 // Follow path to destination 
ey // (veturns ABORT if interrupt or error, OK otherwise) 
1385 char vostr{ STRLEN] ; // Voice string 
1386 int suc{ PLACE UNITS] ; // Succesor list 
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1387 
1388 
1389 
1390 
1391 
1392 
1393 
1394 
1395 
1396 
39 / 
1398 
1399 
1400 
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1402 
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1404 
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1406 
1407 
1408 
1409 
1410 
141] 
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1413 
1414 
1415 
1416 
1417 
1418 
1419 
1420 
1421 
1422 
1423 
1424 
1425 
1426 
1427 
1428 
1429 
1430 
1431 
1432 
1433 
1434 
1435 
1436 
1437 
1438 
1439 
1440 
144] 
1442 
1443 
1444 


otherwise 


into; oy; // Gateway location 

int arrived = 0; // 1 when arrived at destination, 0 
int nav_status = OK; // Navigation status 

int 1; 


behavior mode = NAVIGATION MODE; 


Gout << “Navigating te place [*" << destin << “]%" << endl; 
sprintf(vostr, "Navigating to place %d.\n", destin); 
EK (VOstE);> 


Te ident _seq(); 

ee cout << "Enter current place index ==> "; 

ho Cin >> pnet.windex; 

while((pnet.windex != destin) && (nav_status != ABORT)) { 


pret. find paths(destin, Suc); 
pnet.display(); 


Gout << endl << “Place transition list:” << -end];> 
for {2-> 0; 2) = pnet numevoies; arr) 4 

Coun << "i" << 2 <<") ==> [" (<< sud al) << "1" << endl: 
} 


cout. <= endl? 


if (suc{ pnet.windex] == -1) { 
cout << "navigate seq: No way to get from place [" << 


pnet.windex 


<=") (bo: place [| <<" destin << “)] 2.” =< endl; 
return (ABORT) ; 


if (pnet.link{ pnet.windex][ suc[ pnet.windex]] == NULL) { 
cout << “navigate seq: Nonexistent link [" << pnet.windex 
<a '] =e> [| " << "sue pret. windex) <<."| 2" << endl; 


return (ABORT); 
} 


ae oe} ee 


gx = pnet.link{ pnet.windex] [ suc[ pnet .windex] ] ->gateway x; 

gy = pnet.link[ pnet.windex][ suc{ pnet .windex] ] ->gateway y; 

cout << "Navigating to[" << pnet.windex << "}] --> [" 
<< sucl pnet.windex] << "} gateway at (" << gx << ", ™" << gy 
<< endl; 

nav status = local nav seqigx, gy); 


ker (nay Stacuse.| = ARORT )o{ 
ident seq(); 
} 
} 


if (nav_status == ABORT) { 


COULS=<) “Aborted: <<, endl: 
} 
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1445 
1446 
1447 
1448 
1449 
1450 
145] 
1452 
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1460 
1461 
1462 
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1464 
1465 
1466 
1467 
1468 
1469 
1470 
147] 
1472 
1473 
1474 
1475 
1476 
1477 
1478 
1479 
1480 
1481 
1482 
1483 
1484 
1485 
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1487 
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1493 
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1497 
1498 
1499 
1500 
1501 
1502 


else { 
cout << “Arrivedsat (destination place |* << destin <3) i 7. 


endl; 


} 


} 


HELENE (Navy status); 


int agent::local_ nav_seq(int gx, int gy) // Local destination 
coordinates 


{ 


// Local navigation sequencer 


char vostr[ STRLEN] ; // VoLee string 

double sdist; // Distance from goal 

double min dist; // Minimum distance to goal so far 

double bearing; // Bearing to goal 

int nav status = 0; // 1: arrived, 0: otherwise 

Ine stall count = 0; // Timesteps since progress made toward 
goal 

update (); 


dist =-hypot ((double) (gx = r.x), (deuele Mtg =) 210 40, 
// cout << "Distance from goal =" << dist << " inches" << endl; 


bearing = atan2((double) (gy - r.y), (double) (gx - xr.x)) * RAD2DEG; 
// cout << "Bearing to goal =" << bearing << endl; 


min dist = dist; 


Sprintrt(vostr, “Navigating to ¢d ¢d.\n", ox, oy) 
li, Dei yost. 
COuk << VOSL~E; 


if ((iscan() != ABORT) && (dist > LOCAL NAV _TOLERANCE)) { 
r.face angle fast({int) (bearing * 10.0)); 
} 


while((iscan() != ABORT) && (dist > LOCAL NAV TOLERANCE) && 
(stall count < STALL TIMEOUT)) { 


update (); 


bearing = atan2((double) (gy - r.y), (double) (gx - r.x)) * RADZEEG, 
if (angle diff(bearing, (double) r.theta / 10.0) > LOCAL TIP ANGLE} 


r.face angle fast {tint}; (bearing ~ 10.0)), 
} 
else { 
Set detaults(); 
nav_status = local _navigation_behaviors(gx, gy); 
execute (); 


} 


dist = hypot((doublie) (gx - rix), (double) (qy — ravi 10-G; 
77. cout << "Distance from goal =" << dist << " inches" << endl, 


if (dist < min dist) { 
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} 


Mig cSt = dist, 
stall count = 0; 

} 

else { 
stall counts. 
is, (Stal secoung. = == 0) { 
Sprintt(vostr, "Stalled for ¢d steps:\n", stall.count); 
Cour =< “7oOSstr; 
Gh VOSE LD); 
} 

} 

} 


Sti)? 


if (stall _ count >= STALL TIMEOUT) { 
Sorintt(vostr, "Navigation timeout.\n", 
Stal) count); 
COuG “<<. VOSCr, 
PitVOSter)- 
return( TIMEOUT); 
} 
else if (dist > LOCAL NAV TOLERANCE) { 
@out <<. Aborted.” << endl; 
Ek Aborted.” ); 
return (ABORT); 
} 


cout << “Arrived.” << endl; 
dt Gece ved." ):3 
return (OK); 


int agent::path_local_ nav_seq(path p, // eatin Gomsel1 oq 


{ 


int &waypoint) // Index of next waypoint 


// Local navigation sequencer for path following 


char message{ STRLEN] ; // Message from other robot 

char vostr[ STRLEN] ; // Voice string 

double dist; // Distance from goal 

double min_dist; // Minimum distance to goal so far 

double close dist; // Distance to closest waypoint 

double bearing; // Bearing to goal 

Int Ox; GVe // Waypoint coordinates 

int nav_status = 0; // 1: arrived, 0: otherwise 

itee Stalls count. = 70), // Timesteps since progress made toward 
goal 


int close index = waypoint; // Index of closest waypoint 
Te ey 


// Set goal to next waypoint 


gx = p.x{ waypoint] ; 
gy = p.y{ waypoint] ; 


// Update robot state 
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1561 
1562 
1563 
1564 
1565 
1566 
1567 
1568 
1569 
1570 
ey | 
lo 
le 
1574 
5 
1576 
7 
1578 
leo 
1580 
1581 
1582 
1583 
1584 
1585 
1586 
1587 
1588 
1589 
1590 
1591 
[592 
1593 
1594 
1595 
1596 
lis) 
1598 
1599 
1600 
1601 
1602 
1603 
1604 
1605 
1606 
1607 
1608 
1609 
1610 
1611 
1612 
1613 
1614 
1615 
1616 
1617 
1618 


update(); 
// Find distance/bearing to goal 


dist = hypoti{ (double) (gx = r.x), (double) (qy — fay) ceo- 
// cout << "Distance from goal =" << dist << " inches” << endl; 


bearing = atan2((double) (gy - r.y), (double) (gx - r.x)) * RAD2DEG; 
// cout << "Bearing to goal =" << bearing << endl; 


Mimeorst = cast; 

Sprintiivostr, "Navigating to 44a 2do\n' > cxjigy)> 
pe Ose i), 

Cou << —VOSET; 

// Find distance to closest waypoint 


close dist = closest_waypoint(p, r.x, r.y, waypoint, close index) ; 


while((iscan() != ABORT) && (close dist > LOCAL NAV TOLERANCE) && 
(Stall 2cooOnt . stk TT IMECUT i 


// Update robot state 

update(); 

ij “SOD 1 t “col las ion 

bump _halt(); 

// Realign if turret is misaligned with base 
maintain alignment (); 

// Find bearing to goal 


bearing = atan2((double) (gy - r.y), (double) (gx - r.x)) * RADZBee 


cout << "goal [™ << waypoint <<""] =: bearing = " "<< {bearing 
<<" 4 dist. = " <<“dast <<" «| sevosest | << "close inde. 
<<") 2 dist = << close dist << endl; 


// Orient toward open corridor and advance 


goal corridor_orient(gx, gy); 
corridor advance () ; 


// Check distance from goal 
dist = hypot((double) (gx - r.x), (double) (gy - r.y)) / 10.0; 
if (dist < min dist) { 

// If progress has been made, reset stall counter 


min dist = dist; 
stall _ count = 0; 
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1629 
1630 


} 


} 


else { 
// Otherwise, increment stall counter 


stall counttt+; 

If “stall count % 
Sorinch (7ostu, “ot 
Gout << vostr; 

EK VOStr) ; 

} 


= A)) it 


5 = 
alled for %*d steps.\n", stall_count); 


} 


// Find distance to closest waypoint 


close dist = closest_waypoint(p, r.x, ¥©.y, waypoint, close index); 


} 
// Determine why navigation terminated 


if (stall_count >= STALL TIMEOUT) { // Timeout 
sprintf(vostr, "Navigation timeout.\n", 
Stalt count); 
cout << VOstr; 
Eikt(vostL 
return (TIMEOUT); 
} 
else if (close dist > LOCAL _NAV TOLERANCE) { // User abort 
coun =< “Aborted.” << endl; 
tk ("Aborted."); 
return (ABORT); 
} 


Gout << "Arrived:” << endl: // Success 
Je tic Nrrived.") > 


// Advance to next waypoint on path after closest waypoint 
waypoint = close index + 1; 


return OK 


int agent::local_cont_nav_seq(int gx, int gy) // Local destination 
coords 


{ 


// Local navigation sequencer (continuous motion) 


char vostr[ STRLEN] ; // Voice string 

double dist; // Distance from goal 

double min_dist; // Minimum distance to goal so far 

double bearing; // Bearing to goal 

int nav_status = 0; // 1: arrived, 0: otherwise 

ine stall count — 20; // Timesteps since progress made toward 
goal 

update (); 
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dist = hypot( (double) (g%—. rex), (double) tqy = 24y)) 7 1020; 
// cout << "Distance from goal =" << dist << " inmehes” << endit 


bearing = atanz((deuble) (gy —- r.y), (double) (gx — 22%) * RADZDEGC 
// cout << "Bearing to goal =" << bearing << endl; 


min dist = dist; 


Serinti(vostzr, “Navigating EO sd -ed.\n'| gx, gy); 
/f HE (CVOSEE): 
COourr=< VOStr; 


if (angle diff(bearing, (double) r.theta / 10.0) > LOCAL TIP ANGEE 
fe face angle Fast ( (int) (beering ~ 10 .0)7)7 


} 


while((iscan() != ABORT) && (dist > LOCAL NAV TOLERANCE) && 
(stall count < STALL TIMEOUT)) { 


update (); 


bearing = atan2((double) (gy - r.y), (double) (gx -— r.x)) * RADZDEG 
if (angle diff(bearing, (double) r.theta / 10.0) > LOCAL TIP ANGER) 


eetece angle tfaor (ine) abearing - 10.0); 
} 
else { 
set defaults (); 
nav_ status = local navigation _behaviors(gx, gy); 
execute(); 


} 


dist = hypot((double) (gx - r.x), (double) (gy - r.y)) / 10.0; 
ae cout << "Distance from goal =" << dist << " inches” << ena 


Gn (cast <= mincdise) < 
Mindi St = scuse: 
stall count = 0; 

} 

else { 
Sial Counts ; 
if “(scalleecoun’e Ss 29=— 0) | 
Sprintr(vostr, 7 Stalled for 2d steps. \n , Stall count), 
eon <= Vostr,; 
Ek(Vostr); 
} 

} 

} 


J/ Sti); 


LE (stalivcounts>— siAllL rT IMeEOUT | 
SPrintf(vostr, “Navigation timeout.\n", 
Stall count); 
COUT. << Vostrr;: 
CK(VOSEZ) = 
return (TIMEOUT) ; 
} 
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735 else if (dist > LOCAL NAV TOLERANCE) { 


1736 Coutes< "Aborted." << endl; 

1737 tk("Aborted."); 

1738 return (ABORT) ; 

1739 } 

1740 

1741 cout << "Arrived." << endl; 

1742 ff ENO A era Ved ee 

1743 Peturn OK): 

1744} 

1745 

1746 int agent::local nav_seq alt(int gx, int gy, // Goal coordinates 

1747 inteax, antay) // Alternate goal coordinates 

1748 

1749 // Local navigation sequencer (with alternate goal) 

1750 

1751 char vostr{ STRLEN] ; // Voice string 

ity 2 double dist; // Distance from goal 

He 53 double alt dist; // Distance from alternate goal 

1754 double min_ dist; // Minimum distance to goal so far 

7) double bearing; // Bearing to goal 

1756 int nav: status = 0; // 1: arrived, 0: otherwise 

757 int stall count = 0; // Timesteps since progress made toward 

1758 = goal 

1759 int interrupt; // Interrupt code 

1760 

176] update (); 

1762 

1763 dist = hypot((double) (gx - r.x), (double) (gy - r.y)) / 10.0; 

1764 V7. “cout <<. "Distance trom goal = "<<< dist << " inches” << endl, 

1765 

1766 sleeorst = NypOr (| double) (ax =“=nx), (double) (ay — r24)) 7 1020; 

1767 // cout << "Distance from alternate goal =" << alt dist <<" 

1768 inches" 

1769 Lf <= end; 

1770 

71 bearing = atan2((double) (gy - r.y), (double) (gx - r.x)) * RAD2DEG; 

ae // cout << "Bearing to goal =" << bearing << endl; 

1774 min_dist = dist; 

775 

1776 sprintf(vostr, "Navigating to $d $d.\n", gx, gy); 

mii] 17 Eki vostri 

1778 cout << vostr; 

1779 

roy GCoule =-e Alvernatetqeal.-".<— ax.<< “= << Ayu. 3 44-9 cna, 

eS r.face angle fast((int) (bearing * 10.0)); 

ae while(((interrupt = iscan()) != ABORT) && (dist > LOCAL NAV TOLERANCE) 
& & 

eo (stall_count < STALL TIMEOUT) && (alt dist > LOCAL NAV TOLERANCE) ) 
{ 

1788 

1789 update (); 

1790 

179] bearing = atan2((double) (gy - r.y), (double) (gx - r.x)) * RAD2DEG; 
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1838 
1839 
1840 
184] 
1842 
1843 
1844 
1845 
1846 
1847 
1848 
1849 


if (angle diff(bearing, (double) r.theta / 10.0) > LOCAL TIP ANGLE) 


Er. face angle fase (tint) (bearing ~ 91020); 
} 
else { 
See detaules) 7 
nav_status = local_navigation_behaviors(gx, gy); 
execute (); 


} 


Gist = hypot ( (double) (gx = r.x), (double) (gy — riy)) 7 10.0; 
i cout << "Distance from goal =" << dist << " inches" << endl; 


alt dist = hypot({double) (ax = r.x}, (double) (ay - r-y)) 7 1Omer 

ve cout << “Distance from alternate goal =" << alt dist <2 
inches" 

Vie << ene 


Ve (dase <— Minadsse) 4 
min dist = dist; 
stall count = 0; 

} 

else { 
stall count «7; 
Le (stall teount= 5) —— 0) 
sprintf(vostr, “Stalled for td steps.\n", stall count); 
Cou =< VOSter; 
Ekivosir) 
} 

} 

} 


Sti; 


if (stall count >= STALL TIMEOUT) { 
sprintf(vostr, "Navigation timeout.\n", 
Stall Vcount); 
GOuL << VOStEr, 
CKCVoOStry: 
return ( TIMEOUT) ; 
} 


if (interrupt == ABORT) { 
cout << "Aborted." << endl; 
Eki Bborved.” ); 
return (ABORT) ; 

} 


1f (dist <= LOCAL NAV TOLERANCE) { 
COUL << “Arrived. << endl > 
7 the Z2rrivea.”)-: 
return (On; 


} 


if (alt_dist <= LOCAL NAV TOLERANCE) { 
cout << "Arrived at alternate goal." << endl; 
Tf tk("Arrived at alternate goal."); 
return (ALT) ; 
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1850 
185] 
1852 
1853 
1854 
1855 
1856 
1857 
1858 
1859 
1860 
1861 
1862 
1863 
1864 
1865 
1866 
1867 
1868 
1869 
1870 
187] 
1872 
1873 
1874 
1875 
1876 
1877 
1878 
1879 
1880 
1881 
1882 
1883 
1884 
1885 
1886 
1887 
1888 
1889 
1890 
1891 
1892 
893 
1894 
1895 
1896 
1897 
1898 
1899 
1900 
1901 
1902 
1903 
1904 
1905 
1906 
1907 


} 


} 


Coue = 2o0cal may seq ait. iblegal terminarron.: .<< endl; 
GxXLG (Sa); 


void agent::center seq(void) 


{ 


// Move to center of current place 


oe ee kp Cys // Place center 
int ctheta; // Place orientation 
if (pnet.windex == -1l) { 
cout << "Unable to center at unknown location." << endl; 
return; 
} 
cx = (int) pnet.unit{ pnet.windex] .x; 
cy = (int) pnet.unit[ pnet.windex] .y; 
ctheta = (int) (pnet.unit[ pnet.windex] .theta * 10.0); 
Te cx = 0 
cy = 0; 
ctheta = 0;*/ 
r.move to xy(cx, cy); 
©. tace angle(ctheta); 
// BEGIN SCOUT THESIS CHANGE 
// comment out call for turret alignment - is not necessary for SCOUT 
hie SatLuErertnal toni); 
// END SCOUT THESIS CHANGE 
} 
int agent::path nav _seq(double gx, double gy) // World coords of goal 


{ 


// Navigate to goal by planning and following path 


path nav_path; // Navigation path 
int nav _ status; // Navigation status 
int) path, found; // 1if path found, 0 otherwise 


int next_lls_ point = NAV_LLS SWEEP INTERVAL; // Waypoint for next LLS 


sweep 


eR eed Says 


path_found = path_plan(gx, gy, nav_path); 
Te pacha lound)a 

betinn( NOW AGH); 
} 


// cout << "Press <enter> to continue." << endl; 
ide CADSGee (14 


FOr ie po) DAV pace Penge, 
Mav StaAcUS = Pach. bocal Nay Seq (lavseaes 2). 


// Stop immediately at end of path 
// (so the robot doesn't crash if the goal is next to a wall) 
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1908 
1909 
1910 
191] 
Ie 
Iol3 
1914 
1915 
1916 
Py 
1918 
1919 
1920 
1921 
1922 
1923 
1924 
1925 
1926 
2, 
1928 
0 
1930 
193] 
1932 
1933 
1934 
1935 
1936 
1937 
1938 
1939 
1940 
194] 
1942 
1943 
1944 
1945 
1946 
1947 
1948 
1949 
1950 
1951 
52 
1955 
1954 
1955 
1956 
1957 
1958 
159 
1960 
1961 
1902 
1963 
1964 
1965 


if (i == nav_path.length) { 

SEM) ; 

cout << “Stopping st path ' s- end.” << -endi- 
} 


if (i >= next_lls point) { 
// Sweep laser at intervals (for contloc) 
lls sweep seq(egrid); 


next_lls point += NAV_LLS SWEEP INTERVAL; 
} 


if (nav_status == ABORT) { // User aborted 
return (ABORT); 
} 


if (nav_status == ALT) { // Arrived unexpectedly at goal 
display path(nav_path, TRAV_PATH COLOR, global window) ; 
return (OK); 


} 


if (nav_status == TIMEOUT) { // Navigation timeout 
return (TIMEOUT); 
} 


// Mark traversed path segment in global window 


global window->set_color(TRAV_PATH COLOR); 
EO (= 0 2 oe ee 
global window->display line(nav_path.x[ 3], nav_path.y jl], 
Mav path-xl 7 + 1)7 nav pathy. te) 
} 
global window->flush(); 
global window->set color ( black: ); 


// Mark traversed path segment in robot window 


i for (j= 07 og) < 2 = te ate) 4 


// draw line(nav_ path.x[ 3], nav_path.y jl], 
ie nav_path.x{j + 1], nav_path.yi/j + 1), 
Tf ROBOT TRAV PATH COLOR + 2); 
ff j 
} 
St (); 
return (OK); // Arrived at goal 
} 
int agent::frontier path _nav_seq(int front_index) // Frontier andes 


{ 


// Navigate to frontier by planning and following path 


path nav_path; // Navigation path 

double gx; gy; // World coords of frontier Gentroid 
int nav_status; // Navigation status 

int path found; // 1 if path found, 0 otherwise 

Te we ye 
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1966 
1967 
1968 
1969 
1970 
1971 
ies? 2 
i 73 
1974 
ne? 5 
1976 
1977 
1978 
79 
1980 
1981 
1982 
1983 
1984 
1985 
1986 
1987 
1988 
1989 
1990 
ne | 
ne 
1993 
1994 
NOS 
1996 
1997 
1998 
1999 
2000 
2001 
2002 
2003 
2004 
2005 
2006 
2007 
2008 
2009 
2010 
2011 
2012 
2013 
2014 
2015 
2016 
2017 
2018 
2019 
2020 
2021 
B22 
2023 


frontiers[ front_index] .x; 
Erentsers| front andex) <y; 


gx 
GY 


path _found = frontier_path_plan(gx, gy, front_index, nav_path); 
Et (1? path found) {1 

return (NO PATH); 
} 


update (); 


// cout << "Press <enter> to continue.” << endl; 
i, “Cine gett); 


for (i = 1; i < nav_path.length; ) { 
nav_status = path_local nav_seq(nav_path, i); 
// Stop immediately at end of path 
// (so the robot doesn't crash if the goal is next to a wall) 
if (i == nav_path.length) { 
1 Oe 
COUL << "Stepping at path s end.” <<-endl; 


} 


if (nav_status == ABORT) { {// User aborted 
return (ABORT) ; 
} 


Pei hay Status. —— At) 4 // Arrived unexpectedly at goal 
display path(nav_path, TRAV PATH COLOR, global window) ; 
FeLurn(OK); 

} 


if (nav_status == TIMEOUT) { // Navigation timeout 
Feturn( TIMEOUT); 
} 


// Mark traversed path segment in global window 
global window->set_color(TRAV_ PATH COLOR); 
LOe ee Ol aie te) 
global window->display line(nav_path.x[j], nav_path.y jl], 
Rav = Daken. X[p) ute] aleve ae ey acral, 


} 
global window->flush(); 
Global windew=- seu color ( black); 


// Mark traversed path segment in robot window 


To fOr (jt =_ 0p ey Se He a 


i draw _line(nav_path.x{ Jj], nav_path.y{ j], 
Vs Navepati.x[ 78+ 1) enavopabn yy eats, 
// ROBOT TRAV PATH COLOR + 2); 
pf, &} 
} 
return(OnK) ; // Arrived at goal 
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2024 
202> 
2026 
2027 
2028 
2029 
2030 
2031 
2032 
2033 
2034 
2035 
2036 
2031 
2038 
2039 
2040 
2041 
2042 
2043 
2044 
2045 
2046 
2047 
2048 
2049 
2050 
2051 
2052 
2053 
2054 
2055 
2056 

2057 
2058 
2059 
2060 
2061 
2062 
2063 
2064 
2065 
2066 
2067 
2068 
2069 
2070 
2071 
2072 
2013 
2074 
2075 
2076 
2077 
2078 
2079 
2080 


void agent::sonar_ sweep seq(Map3D map) 


{ 


// Rotate Sonar sensors and scan 
nit oy 


for (i = 0; 
update (); 

// THESIS SCOUT CHANGE send r.theta not r.turret for SCOUT 
sonar scan(somar_smd, sonar clear smd, map, r£-x, ©.y, ©.theta) ae 
// cout << "r.theta= " << r.theta << endl; // show robot heading 

value 

a grid display global (map) ; 
updated display after each scan 


i < SONAR SWEEP WIDTH; i += SONAR SWEEP STEP) { 


// TEMP FIX test map display - shows 


// BEGIN SCOUT THESIS CHANGE 
scout _vm(0, SONAR SWEEP STEP * 
turret - changed pr to vm 
ya WS leer, o.), 
Sleep instead 
sleep (3); // SCOUT THESIS CHANGE added this line as test ** PAUSE 
robot at intervals** 


} 


LO; // Rotate the robot, - not the 


// TEMP FIX comment this line out and try 


// SCOUT THESIS CHANGE - do not rotate Scout back as line below would do 
// hopefully this will decrease the odometry error buildup 
// scoutepr (0, SONARUSWEEP WIDTH ~ =10); // Rotate the robot back 
// ws(l, 1, 0, 5); // TEMP FIX comment this line out and try sleep 
instead 
Va sleep (3); // TEMP FIX added this line as test 
// END SCOUT THESIS CHANGE 

Update); 
} 


void agent::sonar_sweep_abs_seq(Map3D map) 

{ 
// Rotate sonar sensors and scan (absolute coordinates) 
Int. i; 


for (1 = 0; i < SONAR_SWEEP WIDTH; i += SONAR_SWEEP STEP) { 


update (); 

sonar scan_abs(sonar_smd, sonar_clear smd, map, ©.xX, r.y, ©.thevae 
// cout << "r.theta=" << r.theta << endl; // show robot heading 
value 


fa grid display global (map); // TEMP FIX test map display - shows 
updated display after each scan 


// TEMP FIX “send treeheta Moe F.tueret Lor SCOUT 


// BEGIN SCOUT THESIS CHANGE 
scout _vm(0, SONAR _ SWEEP STEP * 

if wst{l, 12°07" 9); 

sleep cmd instead 
sleep(1); if 


z LQ) // changed pr to vm 
// TEMP FIX comment this line out and try 


TEMP FIX added this line as test 
} 
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2081 
2082 
2083 
2084 
2085 
2086 
2087 
2088 
2089 
2090 
2091 
2092 
Z093 
2094 
2095 
2096 
2097 
2098 
2099 
2100 
2101 
2102 
2103 
2104 
2105 
2106 
2107 
2108 
2109 
2110 
2111 
Z112 
eal 3 
2114 
Z115 
2116 
2117 
2118 
£119 
2120 
2121 
7122 
£123 
2124 
2125 
2126 
2127 
2128 
2129 
2130 
Z131 
2132 
2133 
2134 
2135 
2136 
2137 
2138 


// SCOUT THESIS CHANGE - do not rotate Scout back as line below would 


// hopefully this will decrease the odometry error buildup 

// scout_pr(0, SONAR _SWEEP WIDTH * -10); 

tf Vas tle io 05 5) // TEMP FIX comment out this line and try sleep 
cmd instead 


// sleep(3); // TEMP FIX added this line as test 
// END SCOUT THESIS CHANGE 
update (); 


} 


void agent::laser sweep seq(Map3D map) 
// Rotate laser scanner and scan 


{ 


int scans = 0; 


// BEGIN SCOUT THESIS CHANGE 

scout_vm(0, 3600); // just in case we ever put a laser on the Scout 
// TEMP FIX - use vm instead of pr 
// END SCOUT THESIS CHANGE 

Bpoawalt start(); 


while (State[ STATE VEL TURRET] > QO) { 
laser scan(Mmap, ©.x, r.y, r.theta); 
ever has a fixed laser -yeh right 
if (realtime display) { 
display robot (global window, State[{ 34], State{ 35), State 36], 
State[ 37] ); 
} 


scanstt; 


// TEMP FIX for SCOUl tei 


} 


cout << scans << " scans completed : avg scan interval = " 
<< 360.0 / (double) scans << " degrees" << endl; 
} 


void agent::laser sweep abs _seq(Map3D map) 
// Rotate laser scanner and scan (absolute coordinates) 


{ 


int scans = 0; 


// BEGIN SCOUT THESIS CHANGE 

scout _vm(0, 3600); // TEMP FIX - use vm instead of pr 
// END SCOUT THESIS CHANGE 

YaWale Stan )¢ 


while (State[ STATE VEL TURRET] > QO) { 
laser sCaneabs (Map, 1.x, - ¥? os. cneta); 
fixed laser 
if (realtime display) { 
display robot (global window, State[ 34], State 35), State 36], 
State 37]; 
j 


scanstt+; 


//TEMP FIX for SCOUT with 


} 


Coute << ‘scans’ <<." 
<a wos Ores, 


scans completed 
(double) scans << " 


avg scan interval =" 
degrees" << endl; 
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2139 
2140 
2141 
2142 
2143 
2144 
2145 
2146 
2147 
2148 
2149 
2150 
2151 
Zee 
2155 
2154 
ZS) 
ZS 
27 
2196 
2So 
2160 
2161 
2162 
ZG3 
2164 
ZNGS 
2166 
2167 
2168 
2169 
2170 
2171 
2072 
ZEe3 
2174 
ZS 
21746 
2a 
2178 
2a 
2180 
2181 
Ze? 
ZS 
2184 
2185 
2186 
2187 
2188 
ZNO 
2190 
2191 
ZZ 
Z1> 
2194 
ZS 
ZN96 


} 


void agent::lls sweep seq(Map3D map) 
// lLaser-limited sonar sweep 


{ 


int scans = Q; 


// SCOUT NOTE - do not know how Scout handles sp command 
sp(DEFAULT SPEED, DEFAULT _TURN_RATE, 0); // TEMP VE EX ter SCOUL 


r.Seonar single (0), 

Poa Single (0 5; 
// BEGIN SCOUT THESIS CHANGE 

scout_vm(0, 3600); // TEMP FIX- try vm instead of pr commands @mex 
Scour 
// END SCOUT THESIS CHANGE 

eeWalt Stare), 


while (State[ STATE VEL_TURRET] 
lls scan(sonar_smd, 
TEMP FIX -for SCOUT 
if (realtime display) { 
display ~sebetiglosal window, Statel 34), Statel es), state) sem 
State[ 37] ); 
} 


Scans +t. 


> 0) { 


sonar clear _smd, map, r.x, r.theta); ae 


ray, 


} 


cout << scans << " scans completed : 
<< 360.0 -/ (double) 


avg scan interval =" 
scans << " degrees" << endl; 


Gee On), 
e.SOhdr Ont) 7 
beset (delault veloerey(), 


} 


void agent::lls_ sweep _abs_seq(Map3D map) 
// Laser-limited sonar sweep (absolute coordinates) 


{ 


int scans = 0; 


// SCOUT NOTE - do not know how Scout would handle sp command 
sp(DEFAULT SPEED, DEFAULT TURN RATE, QO); // TEMP FIX. for Scour 


r.sonar single(0); 

SeLe single 0); 
// BEGIN SCOUT THESIS CHANGE 

seout vmi(0, 3600); // TEMP FIX - try wm instead of pr commands ic. 
SCOUT 
// END SCOUT THESIS CHANGE 

EWeate Sane); 


while (State[ STATE VEL TURRET] > 0) { 
lis scan_abs(sonar_smd, sonar clear smd, map, ©.x, ©.Y, r.thetajy; 
// ZEVMP File tor Scour 
if (mealtime display) { 
display robot(global window, State[ 34], State[ 35], State[ 36], 
State[ 37] ); 
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Zo / 
2198 
2199 
2200 
2201 
2202 
2203 
2204 
2205 
2206 
2207 
2208 
2209 
2210 
Zo) | 
Zz12 
2213 
2214 
2215 
2216 
2217 
2218 
2219 
2220 
Z22\ 
2222 
2223 
2224 
2225 
2226 
Z22] 
2228 
2229 
2230 
2231 
2232 
2233 
2234 
2235 
2236 
2237 
2238 
2239 
2240 
224] 
2242 
2243 
2244 
2245 
2246 
2247 
2248 
2249 
2250 
E251 
2252 
2253 
2254 


} 


scanst+t; 


} 


cout << scans << " scans completed : avg scan interval = 
<< 360.0 / (double) scans << " degrees" << endl; 


Eade OM) 
r.sonar_on({); 
r.set default velocity (); 


} 
void agent::map_seq(void) 
¥7 Buila local orid 
char vostr{ STRLEN] ; // Voice string 


Siete 

// BEGIN SCOUT THESIS CHANGE 
ws (1, iL 0, oi) 

// END SCOUT THESIS CHANGE 


update (); 
phet-mlace ilearn((couble) rox, (double) x.y, (double). theta / 070); 


J S SPLINE (VOstr, “Building map for solace ¢d.\n",. pret. windex):; 
Pyeeecolt <= VOstr; 
foeeeek(VoOsts) >; 
Grid clear(pner.unzt] pner.windex| .lgrid) > 
clear robot (pnet.unit{ pnet.windex] .lgrid, 0, 0); 
sonar sweep seq(pnet.unit[{ pnet.windex] .lgrid); 
// laser sweep seq(pnet.unit[ pnet.windex] .lgrid); 


grid _display(grid_ window, pnet.unit[ pnet.windex] .lgrid); 


ecouts<<. “Map, complere,” =< endl; 


} 
void agent::ident_ seq(void) 
{ 
// Place identification sequencer 


// Build grid 


r.update(); 
grid *elear(eguid)y 


Glearerobec (eguia, “0.407, 
sonar sweep seq(egrid); 
// laser sweep seq(egrid); 


grid _display(grid window, egrid); 


// Identify grid 
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2259 
2256 
250 
2258 
2259 
2260 
2261 
2262 
2263 
2264 
2265 
2266 
2267 
2268 
2269 
2270 
22 
Zoe 
2273 
2274 
La 
2276 
227) 
2278 
2279 
2280 
2281 
2282 
2283 
2284 
2285 
2286 
2287 
2288 
2289 
2290 
Z| 
Zag 
2225 
2294 
2295 
2296 
229] 
2298 
2299 
2300 
2301 
2302 
2303 
2304 
2205 
2306 
2307 
2308 
2309 
2310 
2311 
2542 


grid ident seqi); 
} 


void agent::grid_ident seq(void) 
{ 


// Grid identification sequencer 


char comm str{ STRLEN] ; 
cnar VOSErM STRUEENI; 
double tx, ty; 

double ttheta; 

1ME) te 2k), Veneta, 
int ident; 


// Communicatwens string 
// Voice string 
// Veanslation. vector 
{J Rotation 
// Identified position 
// Place ident index 


ident = pnet.best_match(egrid) ; 
cout << "Untvanstormed best match = [~ <<-1dent <<") )-- ena 


ident = pnet.best trans match(egrid, tx, ty, ttheta); 
cout << endl; 


cout << "Transformed best match =[" << ident << "] (" << 
pnet.unit{ ident] .x 
aa 4) <= Pere unite 2dent| .y-<<.")” << endl; 


Court << “Transformation. = (7 << tx << 7 


a we 


" << ty << ") [" << tthepoamee 
<< endl; 


// Update dead reckoning 


gs(); 

ie = (ine) (cx *~ £2020 + .0.5)- 

ty = {4nt)> fey skeeO Os ewOr 

itheta = wrapir-theta + tint). ({ctheta * -10-0 + 025), 0, 22-2 

Coue << endl: 

ecout <<“ places— "<< madeneg<< “Veh x = 9 Soe y=" << ly 
<<, theta =" << itheta << endl; 


sprintf(vostr, "I am at place %d.\n", ident); 


EkK(vyostr): 


pnet.windex = ident; 
pnet.display(); 


rox = Is (ant) (pmet ane 2dentii2x + 0.5); 
teye= iy = (inl) (pret ume dent) .y + 0. 5)7 
r.theta = itheta; 

place YObDOE(r.x,; “r.Vret-.theua, retheeay, 


// sprintf(comm_str, "%s/gridsd d $d %d", apndir, 
itheta) ; 

{// “cOue << “Gomm Str = <" <<"comm str << ">" << end, 
// write_comm(COMM CHANNEL, comm_str, strlen(comm_ str) 


ident, 1x, iy, 
+> Le 


// while(read_comm(COMM CHANNEL, comm_str, 80) < 1); 
// cout << "reply = < " << comm str << "5" << endl; 


2572. 


2513 
2314 
2515 
2316 
Zl] 
2318 
Z519 
2320 
p52 | 
p52. 
E523 
2324 
2325 
2326 
p52] 
2328 
2329 
2330 
2331 
E552 
2333 
2334 
2335 
2336 
p55] 
2338 
2559 
2340 
2341 
2342 
2343 
2344 
2345 
2346 
2347 
2348 
2349 
2350 
2351 
E352 
ES) 
2354 
E355 
2356 
B55 7 
2358 
E559 
2360 
2361 
2362 
2363 
2364 
2365 
2366 
2367 
2368 
2369 
2370 


} 


int agent::frontier nav_seq(int front_index) 


// Erontier destination 


index 


{ 


// Navigate to selected frontier 


int nav_status; // Navigation status 
char vostr{ STRLEN] ; // Voice string 


cout << "Navigating to frontier [" << front_index << "] 
<< (2nt) Poutiers| front andex|i x <<", * 
<< (int) frontiers| trent andex).y <<.) << “endl; 


=-— Cenerove 4" 


sprintf(vostr, “Navigating to frontier @d.\n", 
ERA VOSER \e; 


front_index) ; 


// gxid_display_ global(global grid); 
// gxid_ display regions(region_ map); 
// display region_centroids(0.0, 0.0); 
ff sOlSplay TObot Legion cent o1ds(),; 
Mav istatus = Frontier path may seq (front index), 
if (nav_status == ABORT) { 

return (ABORT) ; 
} 


af (nav_status == OK) { 
sprintf(vostr, "Arrived at frontier %d.\n", 
Count << VOsSer; 


front index); 


tkivostr) 
DE (mum Visit == MAX PRONTIERS) { 
cout << "Visited too many frontiers (>" << MAX FRONTIERS << ™).” 
<< endl? 
exit(-1); 


} 


ieO meas VEL MUM) Visi el. x = 
front visit{ num_visit] .y = 
num visitt++; 


} 


frontiers[ front_index] .x; 
frontiers[ front_index] .y; 


LE ( (Nav status == TIMEOUT) ||| (navi status == NO PATH) )-4 
if (nmum_inac > MAX FRONTIERS) { 


Gout <<  "frontrer nav iseq: Too many inaccessible Erontters eee 
MAX PRONTIERS <<"). "<< endl, 
exis), 

j 

sleep(1); 


sprintf(vostr, "Frontier %d is inaccessible.\n", 
ECOUEN = <= VOSEr, 
Git VoOstiry:. 


front (index) 7 


FrOntier copy (front Inac numa ina) , 
num_inact+; 


Frontiers[ front_index] ); 
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2371 
Do 
2373 
2374 
2319 
2376 
Zod 
2378 
239 
2380 
2381 
2302 
2383 
2384 
2385 
2386 
PES 
2388 
2389 
2390 
239] 
Zay2 
Za) 
2394 
2595 
2396 
Zao) 
2398 
2309 
2400 
2401 
2402 
2403 
2404 
2405 
2406 
2407 
2408 
2409 
2410 
2411 
2412 
2413 
2414 
2415 
2416 
2417 
2418 
2419 
2420 
2421 
2422 
2423 
2424 
2425 
2426 
2427 
2428 


} 


} 


return (nav status); 


[RRR RKRKEEKK BEHAVIOR SETS RAKREKKEKEKE / 


int agent::reactive explore behaviors (void) 


{ 


} 


// Behavior set for reactive exploration 


// Returns 1 if new place mapped, 0 otherwise 


int net status = 0; 


advance (); 
avoid)? 
bump halt(); 


NEG Starus — peu. place learnt (| Gouble) <r-*, (couple) Tray, 
(double) r.theta / 10.0); 


if (nei status -¢ NEWAPLACE) { 


Mapesed().; 
return ( 1)? 


} 


return(0); 


int agent::navigation_ behaviors (void) 


{ 


} 


// Behavior set for navigation 


int nav_status; // 1 if active path link exists at current location, 
// O otherwise 


advance (); 

maintain heading(); 
avoid(); 

bump_halt(); 

nav_status = follow path(); 


pnet.place recall (deuble) 22x, 


destin); 


returninavy Status) ; 


(double) r.y, (double) r.theta / 10mg 


int agent::local navigation behaviors(int gx, int gy) 


{ 


// Behavior set for local navigation 


ane nav_status = 0; 


corridor advance(); 
return (nav status); 


es 


ie 


arrived, 0: 
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otherwise 


} 


[et eee eee UTILITY FUNCTIONS **** eH HHH / 


void agent: :reset (void) 


{ 


// Reset position and timer 


de0,, 0); 
da(0, 0); 


timer = O; 


} 
void agent::set_defaults (void) 
// Set default command values 


speed arb->clear(); 
Eusm arb= > Clear ()y, 


} 


void agent:: update (void) 
{ 
// Update robot state and moving obstacles 


cere ae 
if (timer %* 10 == Q) { 
cout << "Time =" << timer << endl; 
Ve power check(); 
Vek if (logfile != NULL) { 
a) *logfile << timer << " " << pnet.num_units << endl; 
V7. } 
} 
r.update ();> 
Vie peor ji-= 07 a= NUM MOS; ata) 
i/ mob list[ i] .update(r.x, r.y); 
lia} 


j7/e elecar robot (glebal grid, Pio ea y); 


if (realtime display) { 


display robot (global window 72 -x,4ray, ©. eheta, ie. tneta); 


FIXeeCreoCOUT 
} 


ce Sonar print (egrrd,. |); 


timert++; 


} 
void agent: :execute (void) 


{ 


// Send commands to robot 


Zo) 


7/7 TEMe 


2487 
2488 
2489 
2490 
249] 
2492 
2493 
2494 
2495 
2496 
2497 
2498 
2499 
2500 
2501 
2502 
2503 
2504 
2505 
2506 
2507 
2508 
2509 
ZO 
2511 
212 
2513 
2514 
2515 
2516 
2517 
2516 
2519 
2520 
2521 
Za22 
2575 
2524 
Ze) 
220 
Zao] 
2528 
2529 
2530 
2 
Za o2 
2523 
2534 
2535 
2536 
DSO) 
2590 
2559 
2540 
2541 
2542 
2543 
2544 


ae 


} 


int speea com, turn com; 


iy speed window.display(speed_ arb->votes) ; 
if turn window. display(turn_arb->votes); 

speed com = (int) speed arb->command() ; 
Pere 


Urn Com =. (ink) (turn arb--conmand |) 
if ((speed_com == 0) && (turn_com == Q)) { 
UPR econ = (ine) (rerand( “RAND TURN, SAND CIURN)» = 1000); 


Gout << “Random turn <" << LUE com << US Mae fence 
} 


r.movetSoced Com, Lurn com); 


home dist += (int) speed_arb->command () ; 


[RR KKK KK KK BEHAVIORS EAEREREKER Hf 


void agent: :bump_halt (void) 


{ 


// Go limp if bumper touched 


// BEGIN SCOUT THESIS CHANGE 
// comment out the code below that was a hack for a bad bumper 
// rearrange code to match original code 


char voOstri STRLEN] ; 


Ta 
yy 


// Voice string 
// Rotation offset for touch sensors 
// Absolute index of tripped bumper 


int GOuch OLESet; 
ait abs touch; 


// int sleepflag = 0; // Do you sleep? 
steht sys 
ie Ae touch. mart) = 0} 4 


} 
} 


lot // robot motors stop 
fOr — 307} 4..< NUM TOUCH, aaa 37 4 
eet eet OU Ce -4) sn 
Sprinti(vostr, “Contacte On bumeer Sd." ; 
cout << vostr << endl; 
EKIVOSEL) = 


a 


} // close if 
} // el@®se for 


Sprinti(vestz, “Sleeping for 2d seconds :", BUMP SEERE); 
COUL ={<-VoOstr <= endl, 
ERIVOStY) 


Sleep (BUMP SLEEP); 
// close if 


// clode bump_halt 


// Below was all hack code for the procedure above 


// HACK! 


On Coyote, ignore multiple bumps on same bumper. 
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2545 
2546 
2547 
2548 
2549 
2550 
2)» | 
ZOD. 
2553 
2554 
>) 
2556 
255 / 
2558 
2559 
2560 
2561 
2562 
2563 
2564 
2565 
2566 
2567 
2568 
2569 
2570 
Z)/ 1 
Pp 
25/3 
2574 
Z5/5 
2576 
m2) / 7] 

2578 
25/9 
2580 
2581 
2582 
2583 
2584 
2585 
2586 
2587 
2588 
2589 
2590 
7591 
Zo92 
E593 
2594 
es 
2596 
Zo] 
2598 
Z599 
2600 
2601 
2602 


fd 

// REMOVE THIS WHEN COYOTE'S BUMPER BOARD IS FIXED 

La 

ce Ouch Grtsel = Wrap{ (ine) W(double) “(ro theta. Yr bumper offset) 
// / (double) BUMPER SEP + 0.5), 

a NUM TOUCH) ; 

// abs touch = wrap(i + touch offset, NUM TOUCH), 

ef 

Ti Lf ({c.2d == 1) || ! bumped] abs touch) ) { 

oe eK Ge 

ca, SPEINEL (VOSEL, "“Conkact, om bUNper 4G." Abs teuch) ; 
To cout << vostr << endl; 

oy: EKIVGster): 

ge bumped[ abs touch] = 1; 

ia sleepflag = 1; 

if } 

ia } 

7 / } 

ae 

Le 

Tif if (sleepflag) { 

vy SpEinut(Vosen, Sleeping le: 20 seconds. ~ BUMP “SLELP); 
77 cout << vostr << endl; 

77 EK(VOSEE) ; 

ye 

// sleep (BUMP SLEEP) ; 

fal } 

// } 

Le} 


// END SCOUT THESIS CHANGE 


// BEGIN SCOUT THESIS CHANGE 

J) eNOUE = the following procedures recoil ‘and bump recoil were werezen 
// for the Nomad 200 but are NOT implemented in the code 

// The major problem with them on the Nomad 200 is misalignment 

// of the turret and the base. 

// They should actually be easier to implement for the Nomad Scout 
// because of it lack of a turret bumpers will always be fixed in 
relation 

V7 Seon ene. robot, 

// Using these would be better than just using the bump halt routine 
// above which only stops the robot but does not get it away from the 
obstacle 


void agent: : recoil (void) 
{ 


j// Ti. COuChea in. forward halt,» move backward ane turn 


double spd; 
double trn; 


if sa. LOuUChsmax (LS... soe 20a 
spd rdrand(=RECOlL SPEED, OO); 
trn rcrand (-KRECOLL: TURN, RECOIL TURN) ; 


Specdaaeb=-VOLe (sed, RECO SPEED es lGUy, (RECOLEWL): 
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2603 
2604 
2605 
2606 
2607 
2608 
2609 
2610 
2611 
2612 
2613 
2614 
2615 
2616 
2617 
2618 
2619 
2620 
2621 
2622 
2623 
2624 
2625 
2626 
2627 
2628 
2629 
2630 
263 1 
2632 
2633 
2634 
2635 
2636 
2637 
2638 
2639 
2640 
2641 
2642 
2643 
2644 
2645 
2646 
2647 
2648 
2649 
2650 
2651 
2652 
2653 
2654 
2655 
2656 
2657 
2658 
2659 
2660 


turn _arb->vote(trn, RECOIL TURN SIGMA, RECOIL WT); 


cout <<" Recorling back... turn =" << eigen 


Le ti jen me endl; 


(speed = " << spd << ", 


} 


Void agente. - bump recor) (void) 
{ 

// If bumper contact, recoil away 
char voser, SERLEN|; 
double rel angle; // Relative angle from robot to bumper contact 
double touch _angle; // Absolute angle from robot to bumper contact 
double tx, ty; // Coords of bumper contact 
INe Contact tlaq = 0,777 Contacte indicarcr 
ime: 


// Voice string 


(0 = no, 1 = yes) 


for (i = 0; i < NUM TOUCH; i++) { 
Tf. (2 -eouch 11). 
IED Ca) // Go limp 


« 


SUEINEL (Vesta, COomcacte On Dumper SG. , 1), 
tkKivoster)> 


// Compute contact angle 


// NOTE - the BUMPER _SEP number here would have to be changed 


iy to accomodate the different bumper pattern of the Scour 
i which is not evenly spaced around the robot 
relvengle — (double) (r > BUMPER SSER) (7 10), 


sprintf(vostr, "Relative angle %.0f.", 
cout << vostr << endl; 
CKHIVOoster)-; 


Ee Tang le), 


nf ((re leangle <—- 7020) || selvangle >= 27020))4 
Jj} Recoil) back Tf Contact. 1s in forward halt of robot 


cout << "<<< RECOTLING BACK” << endl; 
tk Recoiling back." )-; 


// BEGIN SCOUT THESIS CHANGE 

scout vm(-BUMP RECOIL, 0); // TEMP FIX —- change pr to vm 
ey ws(l, 1, 0, 10); TEMP FIX - comment out the wait 
// END SCOUT THESIS CHANGE 

} 

else { 

// Recoil forward if contact is in rear half of robot 


cout << "RECOILING FORWARD >>>" << endl; 
tk("Recoiling forward."); 


// BEGIN SCOUT THESIS CHANGE 

Scout_vm(BUMP_RECOIL, 0); // TEMP FIX - change pr to vm 
Jf weil, 1, OUseet0) TEMP FIX - comment out the wait 
// END SCOUT THESIS CHANGE 

} 
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2661 
2662 
2663 
2664 
2665 
2666 
2667 
2668 
2669 
2670 
2671 
2672 
2673 
2674 
2675 
2676 
2677 
2678 
2679 
2680 
268 1 
2682 
2683 
2684 
2685 
2686 
2687 
2688 
2689 
2690 
2691 
2692 
2693 
2694 
2695 
2696 
2697 
2698 
2699 
2700 
2701 
2702 
Z/03 
2704 
2/05 
2706 
2/07 
2708 
2709 
2710 
Z/ 11 
2712 
2713 
2714 
m7 15 
2716 
Z/17 
2718 


eomncacte slag =), 


break; // Only recoil from one contact 


} 
} 


// Update global grid for all contacts 


ut LeCOntEace Flag) | 
For (is 07 a= NOM TOUCH; 277) 4 
te Aretoucnl-1))i. 4 
// Compute contact position 


// NOTE - the BUMPER_SEP number here would have to be changed 


iy to accomodate the different bumper pattern of the Scout 
Ty which is not evenly spaced around the robot 
rel_ angle = (double) (i * BUMPER_SEP) / 10.0; 


touch angle = angle wrap((double) r.theta / 10.0 + rel angle); 


tx = (double) r.x + ROBOT RADIUS * 120.0 ~*~ cos(touch angle * 
DEG2RAD) ; 

ty = (double) 1r.y"* ROBOT ERADIUS = 120.0 “sin (couch angie = 
DEG2RAD) ; 


// Update global grid 


Set locatson(glosal (grid, Em aZ0Lo, ty 7 120.0,” SONAR FHETCGHT, 
BOS); 
} 
} 


grid display global (global grid); 
} 
} 


void agent: :maintain_ alignment (void) 
{ 


// Realign turret if it is not aligned with base 


double dev; 
ime align turn: 


// Deviation between base and turret angles 
// Turn required to realign turret 


// BEGIN SCOUT THESIS CHANGE 

// fake code into thinking nonexistent turret is aligned with base 
dey — ©.0; // f£1% fer SCOUT 

// dev = angle diff((double) r.theta / 10.0, (double) r.turret / 10.0); 

if (dev > MAX BASE TURRET DEV) { 
tee Realigning= |); 
Si) 


do { 
cout << "REALIGNING: 
<<Wr tubLlerea<-- 


base =" << r.theta << " turret = " 
deviation =" << dev << endl; 


align_turn = 


(int) (angle sgn_diff((double) r.theta / 10.0, 
(deubic er. -Upretc/.1), 0) 
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2709 
2720 
Zen 
Zi 
2125 
2724 
Zi) 
2726 
Zit 
2728 
2729 
2120 
2g) 
yee 
215 
2734 
21> 
2736 
2107 
2738 
27390 
2740 
2741 
2742 
2743 
2744 
2745 
2746 
2747 
2748 
2749 
2750 
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2753 
2754 
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Ze 
2758 
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2760 
2761 
2762 
2/63 
2764 
2765 
2766 
2767 
2768 
2769 
2770 
2771) 
Ny EP 
Zi1> 
2774 
211d 
2776 


* 10.0450 25); 
cout << “Realignment turn = <" << alion tiem << “> <-send.. 


// NOTE - no turret on Scout to align, next two lines are ignored on 
SCOUL 
Scout vimc0, O)};0 077 TEMP PIX for Scour 
wet0, 0; Ueloon. 
update(); 
dev = 0.0; // vide EOE SCOUL 
a) dev = angle diff((double) r.turret / 10.0, (double) r.theta / 
LOSOi 
} 
while (dev > MAX BASE TURRET DEV); 
cout << "Realignment complete: base =" << r.theta << " : turret = " 


<< retuUrrel << ~ = deviation = > << dev << endl: 
// END SCOUT THESIS CHANGE 
} 
} 


int agent: :advance (void) 
{ 

// Move forward unless front is blocked (return 1 if blocked, 0 
otherwise) 


// Minimum forward distance 
// Minimum peripheral distance 
// Desired speed 


int fwd min; 
ine oe rman 
double spd; 


fwd min = r.arc{ FWD] ; 
per min = r.range.min(FWD_RT, FWD_LF); 


if ((fwd min <= ADV STOP DIST) || (per min <= ADV_PER STOP Dist 
speed arb>-vore (0.0, ADV_SPEED SIGMA, ADV SPEED WT); 
retorat |); 


} 


if ((fwd_min > ADV_SLOW DIST) && (per_min > ADV _PER_SLOW DIST)) { 
Specd adrb--vote (ADV SPEED, ADV_SPEED SIGMA, ADV_SPEED WT); 
return. 0)> 


} 
spd = ADV_SPEED; 


if (fwd min <= ADV SLOW DIST) { 
spd = ADV _ SPEED * (double) (fwd min = ADV_STOP_ DIST) / 
(double) (ADV_SLOW_DIST - ADV STOP DIST); 


} 


if ((per_min <= ADV PER SLOW DIST) && 
spd = ADV PER SPEED; 


(spd > ADV_PER_SPEED)) { 
} 


speed _arb->vote(spd, ADV_SPEED SIGMA, ADV SPEED WT); 
return (Q) ; 
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int agent::advance slow(void) 

{ 
// Move forward slowly unless front is blocked 
// (return 1 if blocked, 0 otherwise) 


int fwd min; // Minimum forward distance 


fwd min = r.arc{ FWD] ; 


if (fwd_min > ADV_SLOW_STOP_DIST) { 

speed arb->vote (ADV_SLOW SPEED, ADV_SLOW_SPEED SIGMA, 

ADV_SLOW SPEED WT); 

returntd) > 
} 
else { 

speed_arb->vote(0.0, ADV_SLOW_ SPEED SIGMA, ADV_SLOW_SPEED WT); 
} 

} 


void agent::maintain heading (void) 


{ 


// Maintain current heading 


turn arb--vote(0.0, MH TURN SIGMA, MH TURN WT); 
} 


void agent: :avoid(void) 


{ 
// Avoid nearby obstacles 


dete ls 
double wt; 
double theta; 


// Voting weight for avoidance 
// Obstacle direction 


for (1 = 0; 1 < NUM RANGE; i++) { 
if (r.range{ i] < AVOID DIST) { 
wt = AVOID WT FACTOR * 
(double) (AVOID DIST - r.range[i]) / (double) AVOID DIST; 
theta = r.sensor2theta(i); 
it (theta > Peony 4 
theta -= 360.0; 
} 


turn arb->vote (theta, AVOTDs TURN. SIGMA,. =wt); 


} 


VOLOTageME. savord): Dias  lert( void) 
{ 


// If front is completely blocked, bias avoidance toward the left 


Side 
if (r.range.max(FWD_RT, FWD_LF) > PVOTDE BERS IDS). 
return; 


} 


turn_arb->vote (AVOID BIAS ANGLE, AVOID BIAS SIGMA, AVOID BIAS WT); 
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} 


void agent::avoid bias right (void) 
{ 

// If front is completely blocked, bias avoidance toward the right 
Side 


if (r.range.max(FWD RT, FWD_LF) > AVOID BIAS DIST) { 
reruns; 


} 


turn_arb->vote(-AVOID_ BIAS ANGLE, AVOID BIAS SIGMA, AVOID BIAS WT); 
} 
void agent::follow_wall_ right (void) 
: Jf A irgr wien vont wall 
double fturn; 7/ Follow turn 


if ((r.range.min(BBR, FFR) > FOLLOW MAX ALIGN DIST) || 


(PearGlPwD) <=" bOLEGW STOR DEST); { 
return; 
} 
Jf eout-<< "mini RT, FWD) =<" << rerange-max( Rl; PWD) <- 


Ee (hisanc) BACk RT) 
FOLLOW ABORT)) { 
Levin = FOLLOW TURN UPACTIOR = 
Yedcel SND sk) jy 
turn_arb->vote(fturn, FOLLOW_TURN SIGMA, FOLLOW _WT); 


i= Pearce PWD: RT) ) a6 eC atacl Bi)B)\ -2 


(double). (i arc BACh ar t= 


} 


// cout << "" << endl; 

} 

void agent::follow wall left (void) 

: // Align with right wall 
double. eurmn, 77 Poltow urn 


if ((r.range.min(BBL, FFL) > FOLLOW MAX ALIGN DIST) || 


(r.arc{ FWD] <= FOLLOW STOP DIST)) { 
return; 
} 
// cout << "min(LF,FWD) = <" << r.range.max(LF,FWD) << ">"; 


Le 7( (¥ jorc| BACK. LF] (r.arc[ FWD] > 
FOLLOW _ABORT)) { 

fturn= -FOLLOW TURN FACTOR * 
Yearc| EWR ee) > 


CuUrnm arOerVvVore (icturn, 


f= r,arc, FWDULF)) && 


(double) (r arc) BACK LE 
FOLLOW_TURN_SIGMA, FOLLOW WT); 
} 


// -Gout <<" “<< ‘endl: 


} 
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void agent::maintain_ distance right (void) 


{ 


// Maintain desired distance from right wall 


int, GLonit min, // Minimum right range reading 
double mdturn; // Maintain distance turn 


if ((r.range.min(BBR, FFR) > FOLLOW _MAX ALIGN DIST) || 
(r.arc{ FWD] <= FOLLOW STOP DIST)) { 
return; 


} 
Bro et he r.range.min(BACK, FWD); 
Lf (fight men '= DESTRED DEST) 4 


mdturn = MD TURN FACTOR * (double) (DESIRED DIST - right_min); 
turn_arb->vote(mdturn, MD TURN SIGMA, MD WT); 


i Cour =< “right fin =<" << Licht omin << “= = suring <" << 
cmd{ TURN] 
cy << ">" << endl; 


} 
} 


void agent::maintain distance left (void) 


{ 


// Maintain desired distance from left wall 


int left _min; 
double mdturn; J// Maintain adistance furn 


Lier. range min (BBL, bri) — FOLEGWS MAX ALIGN Dist) 1 
(r.arc| FWD] <= FOLLOW STOP DIST) ) { 
return; 


} 

Veftemin = ©.range.min (FWD, BACK); 

fEv( Lert mim 1] DESERED ADI Sia 
matunn = —-MD TOURNMEACTOR. ~ ss double) (DES REDSDI St sleet min): 
turn _arb=>vote(meaturn, MD TURN SIGMA, MD wi); 


gs COut <<" bere min = = --< lereamin <<]. curning <-6<< end 10n)) 
a << io" So ce endl; 


j 

[eee Go Pa LGD ELON BER AV IO RS Aas Ses eS 
int agent::follow_path (void) 

Pf Tien ton tollew pata 


// Returns 1 if outgoing place link is active, 0 otherwise 


double path_angle; // Angle for navigation 
if (pnet.output_ valid == 0) { 
i Goube <<. Vm Lost. ; == ena. 
return (0); 
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ati 


} 


} 


path angle = angle sgn diff(pnet-.output, (double) r.tcheta / 10,0); 
turn arb->vote(path angle, NAV_SIGMA, NAV_WT * pnet.contf); 


TeLturn (1)? 


agent::detect dest(int destin) 
// Detect arrival at destination 


if (pnet.windex == destin) { 
cout << "Arrived at destination." << endl; 
tk("Arrived at destination." ); 
rerun); 
} 
else { 
return (0); 


} 


void agent::goal orient(int gx, int gy) 


{ 


// Turn toward goal (turn in place if deviation is too high) 


double bearing; 
double goal angle; 


// Bearing from robot to goal 
// Angle between heading and bearing 


bearing = atan2( (double) (gy - r.y);, (double) (qx - r.x)) * RADZEEe 
Le eout. << “goal = (" << gx << ", " << gy << “) =4e0nrenite— oar oe 
Yea 
77 “< Y.y << “) : -aistance,="" 
// << hypot((double) (gy - r.y), (double) (gx ~ r.x)) / 10.0 
yay << " : bearing =" << bearing << endl; 
goal _ angle = angle sgn_diff(bearing, (double) r.theta / 10.0); 
turn_arb->vote(goal_angle, GOAL SIGMA, GOAL WT); 
ve cout << "heading =" << (double) r.theta / 10.0 << " 
goal_ angle =" 
77 << GOal angle <= endl; 


} 


[% &  k  ok k e FILE ACCESS FUNCTIONS Kk KK KKK K f 


void agent::save_net (void) 


{ 


// Save net in directory 


char dirnamef{ STRLEN] ; 


cout << "Enter directory name ==> "; 
cin >> dirname; 


pnet.save_all(dirname) ; 
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3009 
3010 
3011 
3012 
3013 
3014 
3015 
3016 
3017 
3018 
3019 
3020 
3021 
3022 
3023 
3024 
3025 
3026 
3027 
3028 
3029 
3030 
3031 
3032 
3033 
3034 
3035 
3036 
3037 
3038 
3039 
3040 
3041 
3042 
3043 
3044 
3045 
3046 
3047 
3048 
3049 
3050 
3051 
3052 
3053 
3054 
3055 
3056 
3057 
3058 
3059 
3060 
3061 
3062 
3063 
3064 
3065 
3066 


vold ageme: ;- load. net (void) 


{ 


// Load net from directory 


cout << "Enter directory name ==> "; 
Cin =>, dongir; 


pnet.load_all(apndir) ; 
pnet .display(); 
} 


/* * + ee LOCALIZATION FUNCTIONS *****## x / 


double agent::compute range err(int image[ NUM RANGE], vector rinput) 


{ 


// Compute difference between image and range input 


double match_err; 
int err sum-= 0); 
aileen 7 


17 cout << "image/input:error ="; 


for (1.> 07 1 <) NUM -RANGS, ir {| 
ere sum +> abs (image i] = “ranputl i); 


jae Gout << imagel. i] << "/" << rinput[ a) <<."<" << 

ty abs (image[ i] - rinput[i]) << " "; 
} 

// cout << endl; 
match_err = (double) err_sum / (double) (NUM_RANGE * MAX RANGE) ; 
COuL << “Maten -ermer = ~ << matcn crn << Jendl; 


EFeeUcIiMaee werr); 


} 


[aa RAN ASTRA ee EV EDENCE GRID DISPLAY PUNCTIONS 


eke oe eee oe ae ke ae ok ak ok ee a ok e/ 


void agent::grid display (window *win, // Window pointer 
Map3D map) // Evidence grid 
{ 


// Display evidence grid in X window 


double xa; yd; // Display coords 


double xscale, yscale, zscale; // Cell dimensions (tenths of 
inches) 

Lem; Vy 2 // Cell index 

int xsize, ysize, zsize; // Grid dimensions (# cells) 


aol |S) // Occupancy probability 
win->clear window(); 
xSize = map.msize[ 0] ; 
ysize = map.msizel[ 1] ; 
zsize = map.msizel[ 2] ; 
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xscale = (map.himvi 0) - map.lomvj 0) ) * 120.0 / (double) xsize; 
yscale = (map.himvj 1] - map.lomvj 1])) * 120.0 / (double) ysize; 
zscale = (map.himv{ 2) - map.lomvj 2] ) * 120.0 / (double) zsize; 
// cout << "Disdlaying grid (" << xsize <<" x" << ysize..— ") se 
zsize 
// << ") : scale = (" << xscale << ", " << yscale << "=" << zgseale 
—<— iT Ns 
// << endl; 
z = (int) ((SONAR HEIGHT + HEIGHT OFFSET - map.lomv{ 2]) / 
(map.himv{ 2] - map.lomv{ 2] ) * zsize); 
for (y= 0; -y < vsize, sy). 
for (x = O; x < xsize; xtt) { 


p = map.mapn[ z * xsize * ysize + y * xsize + x); 


xd 
yd 


((double)“(x-+"0.5) * xscale + man.lomui Ol .* 1207 Ci: 
((double) (y+ 0.5) * yscale + map lomy Il = 71Z0.c. 


| 


if (p> 0) { 
win->display circle(xd, yd, xscale / 4.0); 
} 
else 1f (p == 0) { 
win=-display pointe tad, ya). 
} 


ae ft (o>— GRID POS THRESH) at 
Win dEespilay vei rele( xd, yo, “xscale.77- 4.0); 

} 

else if (p > GRID NEG THRESH) { 

nef aoe ar) 
Wine SetColor ( oiue- |} 
win->display point (xd, yd); 
Wins Set ecColory” black! ); 

} 

else if (p < 0) { 
WiIN=- Seu COLOr( red: Jc, 
win->display point(xd, yd); 
Wil Ser .cOPOEt  eilack’ ); 

} 

else { 
win->display point(xd, yd); 

} 

ey 


} 
} 


win->draw_arc buffer(); 

win->flush(); 
} 
void agent::grid_ display global (Map3D map) // Evidence grid 
{ 


// Display global evidence grid in X window 


double xd, yd; // Display coords 
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Ba) 
3126 
Ba / 
3128 
B29 
3130 
B13] 
B132 
B133 
3134 
B135 
3136 
B13 7 
3138 
6139 
3140 
314] 
3142 
3143 
3144 
3145 
3146 
3147 
3148 
3149 
3150 
3151 
Bo 2 
p53 
3154 
B155 
3156 
3157 
3158 
pil59 
3160 
3161 
3162 
3163 
3164 
3165 
3166 
3167 
3168 
3169 
3170 
p17) 
B1/2 
3173 
3174 
B75 
3176 
B77 
3178 
3179 
3180 
3181 
3182 


double xscale, yscale, zscale; // Cell dimensions (tenths of 
inches) 

era apes 7 ec // Cell index 

int xsize, ysize, zsize; // Grid dimensions (# cells) 

Lae. // Occupancy probability 


global window->clear window() ; 


Bor (x 


xsize = map.msizef 0]; 

ysize = map.msizef{ 1]; 

zsize = map.msizef 2] ; 

xscale = (map.himv[ 0] - map.lomvj 0] ) * 120.0 / (double) xsize; 

yscale = (map.himv[i 1) - map.lomvj 1] ) * 120.0 / (double) ysize; 

zscale = (map.himv[ 2] - map.lomv[ 2) ) * 120.0 / (double) zsize; 

Cout << "Displaying Grid (" <<" xsize << "ox" << ysize <a x (22 
zsize 

<< “) * Scale = (" << xscale << ", " << yscalle <<", " << z2scale << 
f Ney 
<< endl; 
Ze ant) (SONAR OPE IGHT = HEIGHT OFFSET = map. lomy 2) ) 7 


(map.himv[ 2) - map.lomv{[ 2] ) * zsize); 


(y= 0s yy < ysize; yr) { 
= 0; x < xsize; x++) { 
p = map.mapm{ z * xsize * ysize + y * xsize + x]; 


({double) (x + 0.5) * xscale + map.lomvyj 0] * 120.0); 
((double| (y + O25). * vseale — map. tom. 1) = 12070); 


< 
Q 
H tl 


ie (210) 4 
global window->display circle(xd, yd, xscale / 4.0); 
} 
else if (p == 0) { 
global window->display point(xd, yd); 
} 


/* if (p >= GRID POS THRESH) { 
global _window->display circle(xd, yd, xscale / 4.0); 

} 

else if (p > GRID NEG THRESH) { 

te olose 04 
Global windows set scolori blue), 
global window->display point(xd, yd); 
global window=>set color( biack;) )- 

} 

else if (p < 0) { 
Global wandow=>set.color(= xed: |; 
global window->display point(xd, yd); 
global window->set color(" black"); 

} 

else i 
global window->display point(xd, yd); 

} 

jee 
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3183 
3184 
3185 
3186 
3187 
3188 
3189 
3190 
3191 
bo 
3193 
3194 
3195 
3196 
3197 
3198 
3199 
3200 
3201 
3202 
3203 
3204 
3205 
3206 
3207 
3208 
3209 
3210 
52) 
32)2 
323 
3214 
3215 
3216 
Say 
3218 
S219 
3220 
322] 
32727 
S225 
3224 
3225 
3226 
322/ 
3228 
a229 
3230 
32311 
B92 0) 
SS 
3234 
S225 
3236 
S237 
3238 
3239 
3240 


} 


} 
} 


global _window->draw arc buffer(); 
global window->flush(); 


Glebal “retzresh = 7; 


void agent: :display place grid(void) 


{ 


} 


VO 


{ 


in 


// 
Zs 


cf 


<< 


La 


// Display local grid for place 


int index; // Place index 


Tf (pret anum wames j= 0 jt 
cout << "No places in APN." << endl; 
return; 
} 
cout << "Enter place index [0.." << pnet.num units - 1 << "] =—=- 38 


cin >> index; 


Lf Clindex: <0) J sandex >= pret nunountes) 4 
cout << "Nonexistent place." << endl; 
SeLuLrn; 


} 


Grid display(grid window, pnet -unac| andex] -igri1d) ; 


id agent::grid_display edges(int grid{ GLOBAL X RES][ GLOBAL Y RES] ) 
// Colored grid 
// Display edge segments detected in evidence grid 


double xd, yd; // Display coords 


double xscale, yscale; // Cell dimensions {tenths cof 
ches) 
aie ye // Cell index 
int xsize, ysize; // Grid dimensions (# cells) 
ince oe // Occupancy probability 
KSi,Z26 = 2GLOBAL) xX RES, 
ysize = GLOBAL Y RES; 
xscale = (GLOBAL X_MAX - GLOBAL X MIN) * 120.0 / (double) xsize; 
yscale = (GLOBAL Y MAX - GLOBAL Y MIN) * 120.0 / (double) ysize; 

cout << “Displaying grid (“<< xsize << " x” << “yvsize <<" 3) 
1ze 

<< ") : scale = (" << xscale << ", " << yscale << ", " << zscale 
tt a 
<< endl; 


global window->set_color(EDGE COLOR); 
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324] 
3242 
3243 
3244 
3245 
3246 
3247 
3248 
3249 
3250 
B21 
B22 
3253 
3254 
3255 
3256 
B2>/ 
3258 
B259 
3260 
3261 
3262 
3263 
3264 
3265 
3266 
3267 
3268 
3269 
3270 
B27] 
B2/2 
B273 
3274 
3275 
3276 
B27 7 
3278 
3279 
3280 
3281 
3282 
3283 
3284 
3285 
3286 
3287 
3288 
3289 
3290 
B29 1 
B292 
3293 
3294 
B295 
3296 
B29] 
3298 


} 
} 


(y = 0; y < ysize; ytt) { 
= 0; x < xsize; xt+t) { 
Pp =-grid xt 


Cl 
yd 


(double) ite + 055)" xscale + ClLOBAL AwMiN.* 12020; 
(double) ty + Oc) *“yecale = Global Yo MIN 4 12020; 


LED 0) 
global window->display circle(xd, yd, xscale / 4.0); 
} 


global window->set_ color ("black"); 


global window->draw arc buffer(); 
global window->flush(); 


} 


void agent::grid_ display regions(int grid{ GLOBAL X_RES][ GLOBAL Y RES] ) 


{ 


7/ Colored grid 


// Display regions detected in evidence grid 


double xd, yd; // Display coords 

double xscale, yscale; // Cell dimensions (tenths of 
inches) 

Drees // Cell index 

int xsize, ysize; // Grid dimensions (# cells) 

ies pe // Occupancy probability 

xsize GLOBAL X RES; 


ysize GLOBAL Y RES; 
xscale = (GLOBAL X MAX - GLOBAL X MIN) * 120.0 / (double) xsize; 
yscale = (GLOBAL Y MAX - GLOBAL Y MIN) * 120.0 / (double) ysize; 
V7 Feout <= 9 Pisolaying wrid it" << x51 ze << “x <2 Vorze << es ' <a 
zsize 
77] << ") ;: scale = (" << xscale << ", " << yscale << ", " << zscale 
eS 
ys << endl; 
for (x = 0; x < xsize; x+tt) { 
fer (vy = 07 y < size; yrt) a 
P= Grid viz 
xd = (double) (x + 0.5) * xscale t CLOBAL ACMING += 120720; 
Vdw= (double) (y #2075) *Syscale wr GEOBAE SY MING*, 12070; 
oe ee et 
Global ewindows>seutcolor(coloretabilec Sigrid x) Vip ael)4s 
DISPLAY COLORS} ); 
he couts<< “display color.— " 
7, <<, cOlorscablel {orld x) (ieee DISPLAY COLORS] << 


endl; 


global window->display circle(xd, yd, xscale / 4.0); 
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3299 
3300 
S501 
2502 
3303 
3304 
S205 
3306 
3307 
3308 
3309 
3310 
aon 
a0 12 
3313 
3314 
Soils 
3316 
3a) 
3318 
3319 
3320 
3321 
3322 
3325 
3324 
S305 
3326 
352) 
3328 
3329 
3330 
3331 
5502 
3333 
3334 
3335 
3336 
5557 
3338 
3352 
3340 
3341 
3342 
3343 
3344 
3345 
3346 
3347 
3348 
3349 
3350 
3351 
So57 
3353 
3354 
3355 
3356 


} 


global window->set_color ("black"); 


} 
} 


global _window->draw_arc_ buffer(); 
global window->flush(); 


void agent::display robot (window *win, // Window 
int <<) Ine. Vy; /7 Robot position. (1/10 ine 
int theta, // Robot heading (1/10 degree) 


{ 


int turret) // Robot turret angle (1/10 degree) 
// Display robot in window 


// Gecal. constants 


const double robot rad = ROBOT RADIUS * 120.0; // Robot radius (1/10 
Ine ky 

const double half rad = robot rad * 0.5; // Half radius (27m 
inen) 

const double tendeg = 0.1 * DEG2RAD; // 1/10 degree in 
radians 

static double old _ fx, old_fy; // Old robot position 

static double old ftheta; // Old robot heading 

Static ‘double old -fturzer, j/7/ Old Frobct turret angle 

static double old bx, old by; // Old endpoint of base line 

Static double ola tx “oldra. // Old endpoints of turret line 


Gldwtx2,, eld £72; 


double 
double 
double 
double 
double 


// Robot position (floating point) 

// Robot heading (floating point) 

// Robot turret angle (floating point) 
// Endpoint of base line 

ty2Z; «/ EMGpoimts or Eurret line 


Poco. 
ftheta; 
fturret; 
Dx, by; 
xy yy see, 
fx 
fy = 
ftheta = 
fturret = 


(double) x; 
(double) y; 
(double) 
(double) 


theta; 
turret; 


bx 
by = 


fx + cos(ftheta * 
fy + sain(teheta * 


tendeg) * 
tendeg) * 


hate (rac; 
half rad; 


txl = £x + 
Cyt foe 


half rad; 
ali vrac: 


COS{TELCUrreEE: * 
Sim (fturre. ~ 


tendeg) 
tendeg) * 


tx2 
Ey2Z 


fx + 
Ly 


COS(TEMELCLE * 
Sin titurrer ~* 


tendeg) 
tendeg) * 


robot_rad; 
FObOt rag, 


it ('olopalerefiresn) { 
Win=>display xon circle (old fx, old fy, ropet rad); 
win--Gisplay xor line (oldirx, old ty, old bx, Glad by); 
win->display xorsline (old™exi, jolid styl, voldvex2, olgtey2), 

} 

global refresh = 0; 
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BoD / 
3358 
BBDo 
3360 
3361 
3362 
3363 
3364 
3365 
3366 
B36 / 
3368 
3369 
3370 
23 / | 
3372 
3373 
3374 
Bo /5 
3376 
Bo] / 
3378 
B3/9 
3380 
3381 
3382 
3383 
3384 
3385 
3386 
3387 
3388 
BOOo 
3390 
53 | 
B92 
3393 
3394 
B39) 
3396 
8397 
3398 
B39 
3400 
3401 
3402 
3403 
3404 
3405 
3406 
3407 
3408 
3409 
3410 
3411 
3412 
3413 
3414 


} 


LRRRERKKEKKEKKKEKKKK KKK 


void agent::frontier copy(frontier &f1, 


{ 


win->display xor circle(fx, fy, robot_rad); 
Win-sausplay XOr 1ine(ix,. Ly, Dx, by); 
WiN=-Cisplay xkKOr Tine(exi, “Eyl exc, Eye); 


win->flush(); 


old. tx = £x; 

Solalty. = £Y; 

eld rtheta = Liheta, 
eld, PEUEFGe = LEUrceL, 
Olid bx = bx, 

Sldsoy = Dy; 

old tx] = txl; 

olastyl = tyl; 

UGE x 2 = “EX2; 

StastyZ = tyzs 


FRONTIER FUNCTIONS KKEKKKHKEKKKKKEKKKKKKEK | 
frontier £2) 


Vi sGOpyY Grontier ~f2-—1:6 fronvier <1 1- 


Plex =) EZ x; 

iyi i Ay; 

fl.size = f2.size; 
LIC OLOnM Shire. COLOr, 


Mord agent: ; Lind Eromeiers( void) 


} 


Void agént::find frontier edges (Map3b “raw, 
(pointer) 


{ 


// Find frontiers in global grid 


bimd GrOnc1er edqes(¢gqlobal grid, sedgevorid, COND Ree Lenn 
find frontier_regions(edge_ grid, SONAR_HEIGHT) ; 

(7) Gt Gecdisplayeguebal (Global Gg iad); 
gGuidvdisplay regions (reqicon map); 

display vecqion cencroids(0.0,,0.0)7 

// display robot region _centroids(); 


// Raw evidence grid 


Map3D *edge, // Frontier edge grid 


(pointer) 


double height) // Z-coord of edge plane 


// Find frontier edges in <raw> grid and store them in <edge> grid 


int xsize, ysize, zsize; // Grid dimensions (# cells) 
Diep sce: aver ace // Cell index 

Lata; // Occupancy probability 

emits lakes // Unknown neighbor flag (0 = true) 
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3415 
3416 
3417 
3418 
3419 
3420 
3421 
3422 
3423 
3424 
3425 
3426 
3427 
3428 
3429 
3430 
3431 
3432 
3433 
3434 
3435 
3436 
3437 
3438 
3439 
3440 
3441 
3442 
3443 
3444 
3445 
3446 
3447 
3448 
3449 
3450 
3451] 
3452 
3453 
3454 
3455 
3456 
3457 
3458 
3459 
3460 
3461 
3462 
3463 
3464 
3465 
3466 
3467 
3468 
3469 
3470 
3471 
3472 


xsize = raw->msizel 0]; 
ysize = raw->msize{ 1] ; 
zsize = raw->msize[ 2] ; 


if ((xsize != edge->msize[ 0] ) || (ysize != edge->msize[{ 1] ) || 
(zsize != edge->msize[ 2] )) { 
Coue << Land frontier eqges: Grid size mi smacen.. << ena, 
Levunn, 


z = (int) ((height + HEIGHT OFFSET - raw->lomvj[ 2]) / 
(raw->himv{ 2] - raw->lomvi 2] ) * zsize); 
for (x = 1; x < xsize - 1; x++) { 
fOr y= 13 “Veo ssoize =—"1lp vt) { 
edge->mapn[ z * xsize * ysize + y * xsize + x) = Q; 
p = raw->mapm[ z * xsize * ysize + y * xsize + xj); 


i ier Ot 
// unk = 0 if and only if one of cell (x,y)'s neighbors is unknown 


unk =<craw->mapml 2 ~*~ XSize * yvsize +-~y * Msize + ss eli 
raw->mapm[ z * xsSize * ysize + y * xsize + x + 1) * 
raw->mapn[ z * xsize * ysize + (y - 1) * xsize + x] * 
raw->mapnm[ z * xsize * ysize + (y + 1) * xsize + xj; 


ie 4nks =—. 0) 
edge->mapn[ z * xsize * ysize + y * xsize + x) = 1; 
} 
} 
/* if (p <= GRID NEG THRESH) { 
if (((raw->mapn[ z * xsize * ysize + y * xsize + x - 1] 


> GRID NEG THRESH) && 
(raw->mapm[ z * xsize * ysize + y * xsize + x - 1J 
< GRID POS THRESH)) || 
((raw->mapm[ z * xsize * ysize + y * xsize + x + ]Jj 
2 GRIDUNEG THRESH) “&é 
(raw->mapm[ z * xsize * ysize + y * xsize + x + 1) 
< GRID POS THRESH)) || 
((raw->mapm[ z2 * xsize * ysize + (y - 1) * xsize + x] 
> GRID NEG THRESH) && 
(raw->mapm[ z * xsize * ysize + (y - 1) * xsize + x] 
< GRID FOS: THRESH) ||) 
((raw->mapn[ z * xsize * ysize + (y + 1) * xsize + x) 
> GRID NEG THRESH) && 
(raw->mapn[ z * xsize * ysize + (y + 1) * xsize + x] 
< GRID POS THRESH))) { 
edge->mapm[ z * xsize * ysize + y * xsize + x) = 1; 


og 
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3473 
3474 
3475 
3476 
3477 
3478 
3479 
3480 
3481 
3482 
3483 
3484 
3485 
3486 
3487 
3488 
3489 
3490 
3491 
3492 
3493 
3494 
3495 
3496 
3497 
3498 
3499 
3500 
3501 
3502 
3503 
3504 
3505 
3506 
3507 
3508 
3509 
3510 
Bl] 
B512 
B13 
3514 
3515 
3516 
p17 
3518 
BO19 
3520 
521 
B22 
B23 
3524 
B25 
3526 
Bo. / 
3528 
5529 
B530 


void agent::find frontier regions (Map3D edge, // Frontier edge 
grid 

double height) // Z-coord of edge plane 
{ 


// Find frontier regions in <edge> grid and add new frontiers 


spread segment (edge, region_map, height); 
analyze regions (region_map); 


} 


void agent::spread segment (Map3D grid, // Uncolored grid 
int color{ GLOBAL X_RES){ GLOBAL Y RES], 
// Colored grid 
double height) // Z-coord of edge plane 
{ 
// Segment <grid> image into regions in <color> using spreading 
activation 


cite oe eee // Cell index 

tat nx, ny: // Neighboring cell index 

ing num colors = 1; // Number of colors 

int XSize, VYSize, ZSize; // Grid dimensions (# cells) 

int, changed; // Flag indicating whether cell colors 
changed 


// Find grid dimensions 


xXxSize = grid.msizef 0] ; 
ysize = grid.msize[ 1]; 
zsize = grid.msizel[ 2] ; 
Ze = fine) (theighie a HEIGH? OLhSE? — grid. tomy, 2). / 


(Grid nimvyi. 2) = grid.domvy2))) “ zsize}: 


¥7/ Ser inaenal colors 


for (x = 0; x < xsize; x++) { 
for (y = 0; y < ysize; ytt) { 
Vo Agra mapnl 2 xsize = ysize t+ y * xsize + «| == 0) { 


Color xj iy) = 0; 
} 
else { 
color xj) y) = mumicolors, 
NUM COlLors i; 
} 
} 
} 


// Use spreading activation to segment regions 


dor{ 

changed = 0; 
for (x = 0; x < xsize; xt+t) { 

LOE Ve— U7 Ay. =) VSize;e Vth) 4 

for (nx = x - 1; nx <= x + 1; nx++) { 

for (ny = y = le ny <= y + lee nytt) 4 
if ((nmx >= 0) && (nx < GLOBAL X RES) && 
(ny >= 0) && (ny < GLOBAL Y RES)) { 


Zio 


> 0) && 
color, mx) | my: + 


ief> 4. (eo Lox) mei [ony] 
colon <iLyi = 
changed = 1; 


while (changed) ; 


} 


{ 


void agent::print region _map(int grid{ GLOBAL X RES][ GLOBAL Y_ RES] ) 


ij? Colored Grid 


// Print colored grid cell values 


char symbol; 


Lae 


// Cell symbol 


xe Ve // Cell index 


cout << endl; 


EOr 


(x=40; x = GLOBAM IX RBS sae) 4 
for (y = 0; y < GLOBAL Y_ RES; yt+t) { 
ee meee Cl Sau) en) a 
Sout, 0 5. 
} 
else { 
ie (gids hy) <0 | 
symbol = °O" + (char) grid ii yi; 
} 
else if (grid xii viv < 36) { 
symbol =A’ + (ehaw) (quid) x) [yi =. LO): 
} 
else { 
Symbol — "a “faichar) (qcid <i vii — soe 


} 


} 
cout << symbol; 
} 


cout << endl; 


} 


cout << endl> 


} 


{ 


void agent::analyze regions(int grid{[ GLOBAL X RES][ GLOBAL Y_ RES] ) 


ji Colored -oraid 


// Determine size and centroid of frontier regions 


double xscale, 


yscale; // Cell dimensions (tenths of 


inches) 


double cx, Cy; 


Pie 
ine 
int 
int 
ae 


// Centroid of new region 
// Edge cell counter for regions 
// Sum of cell x-coords 
// Sum of cell y-coords 

// Cell index 


count[ MAX COLORS] ; 
x sum[ MAX COLORS] ; 
y_sum{ MAX COLORS] ; 
ay oye 

ol 
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(colorink) Envy = colon s)itview 


{ 


3589 
3590 
3591 
B92 
B03 
3594 
Do? 
3596 
Boo / 
3598 
B99 
3600 
3601 
3602 
3603 
3604 
3605 
3606 
3607 
3608 
3609 
3610 
3611 
3612 
3613 
3614 
3615 
3616 
3617 
3618 
3619 
3620 
3621 
3622 
3623 
3624 
3625 
3626 
3627 
3628 
3629 
3630 
3631 
3632 
3633 
3634 
BO35 
3636 
3637 
3638 
3639 
3640 
3641 
3642 
3643 
3644 
3645 
3646 


numyteent = 0; 

xscale = (GLOBAL X MAX - GLOBAL X MIN) * 120.0 / (double) 
GLOBAL X_RES; 

yscale = (GLOBAL Y MAX - GLOBAL Y MIN) * 120.0 / (double) 


GLOBAL Y RES; 


for (i = 0; i < MAX COLORS; i++) { 


count{ i] = 0; 
SSuMiel] = 20; 
y_sum[ i] = 0; 
} 
fora (s 


= 0; x < GLOBAL X RES; xt+t+) { 
for (y = 0; y < GLOBAL Y RES; yt+t) { 
if (grid x][ y] > 0) { 


count[ grid{ x] [ y]] ++; 
x_sum[ grid{ x][y]] += x; 
y_sum{ grid{ x][ y]] += y; 


} 
} 
} 


for (1 = 1; 1 < MAK COLORS; itt) { 
if (count[ i] >= MIN REGION SIZE) { 

cx = 

xscale * (double) x_sumf i] / (double) count[{ i] + GLOBAL X MIN * 
eZ05107 

cy = 

yscale * (double) y_sumi] / (double) count[ i] + GLOBAL Y MIN * 
120.0; 


ae (!ivislreed(ex,. cy). || tmaccessibletcx, cy))) 4 
yi if (linaeccessibleiex, icy) ) { 
if (num_front == MAX FRONTIERS) { 
cout << "analyze regions: Too many regions (>" << MAX FRONTIERS 
<a er ee eric, 
} 
else { 
frontiers| num_ front] .color = 1; 
frontiers{ num_front] .size = count[ i] ; 
feeontiers! num front) .x ex; 
FEOnNELers| num Eront)., ¥ Cy; 
RUM EEOME Ty 
} 
} 


} 
} 


fone 3 = 07. 1 < Mune prone; et) 4 
Couts<< “Regions <<144<<. |) Sizer) Y<-< front ers a) asize 
aoe = ‘CGEntrolcdu—-\(' oSekrOnGlers|) Hi .cc eye So MT rOneier ci 
<< ")" << endl; 
} 
} 


int agent::visited(double cx, double cy) // Centroid of new region 
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3699 
3700 
370] 
3702 
3703 
3704 


//{/ Check whether centroid corresponds to previously visited frontier 
// Return 1 if visited, O otherwise 


double dist; // Distance from new region to visited frontier 


inte 2° 
// cout << "Cheeking (" << cx << ", "<< cy << ") against. vismeed 
Pirsee 
7? << endl; 
for ii = 070 1 =< numivisit,. 17+ 
gust = NVpoe(ex = Trent 271Sit( a) ax, Cy = Frome Visiel aloe 
ie cout << "front visit[" << i << "] at (" << front visit] a) 
Tf << front Visita) .y << ") <dastance =" "<< “dist, 
if (dist <= VISIT RADIUS) { 
jae Gout. <<" [|— VICTTED —|" << endl; 
return(l1); 
} 
es cout << endl; 
} 
reeurn (0). 
} 
int agent::inaccessible(double cx, double cy) // Centroid of new 


region 

{ 
// Check whether centroid corresponds to inaccessible frontier 
// Return 1 if inaccessible, 0 otherwise 


double dist; 
frontier 
inc 1¢ 


// Distance from new region to inaccessible 


ii SCout << "Checking ce << Cx g< ee 


inaccessible list." 


mee sey <a 2) Sagaimsce 


ye << endl; 
for (i = 07 1o< numinaews its) 4 
GlSE = hypoe(cx = EEOne inac| i) ox, cy = front nach aie 
V7 cout << "front inacl ” << a << ") at {(" <<. front inad 1) eae 
TF ._< Erene tnacia)..y <<s") 9: distance = * s—dict. 
if (dist <= INAC RADIUS) { 
1% cout << "Sl = VD INACeeSSIBuE. *)""<<“endL. 
return (1); 
} 
ed cout << endl; 
} 
return(0O); 


} 


int agent::closest_ frontier(double x, double y) 
{ 


// Return index of unvisited, accessible frontier closest to (x, y) 


2/6 


3705 
3706 
3707 
3708 
3709 
3710 
Bil] 
B12 
B13 
3714 
B7i5 
3716 
Bil] 
3718 
B/19 
3720 
5/21 
5/22 
3723 
3724 
B25 
3726 
pi2/ 
3728 
B29 
3730 
Bi/> | 
B/32 
B/33 
3734 
B/35 
3736 
3737 
3738 
B39 
3740 
374] 
3742 
3743 
3744 
3745 
3746 
3747 
3748 
3749 
3750 
3751 
3752 
3753 
3754 
B755 
3756 
5/57 
3758 
B7 59 
3760 
3761 
3762 


} 


// Return =) if no such frontier exists 


double min_dist = MAX DIST; // Minimum distance to frontier 


double dist = -1; /7f DPestance Lo wrontier 
int close index = -1l; // Index of closest frontier 
arf eomeliys 

03 in num erone, tt) 


for (1 = 
ie (l(vrstred( frontiersl a) .%, f£ronciers| ijmy) iJ 
inaccessible(frontiers[ i] .x, frontiers[ i] .y))) { 
a if (!inaccessible(frontiers[ i] .x, frontiers[ i] .y)) { 
Gist’= hypct(x = fronciers| ab .x, y = Lrontiers| i). y); 
if (dist < min Gist jf 
Man ctos = aaisiG ; 
close _index = i; 
} 
} 
} 


return(close_ index); 


void agent::display region _centroids(double cx, // Display center x- 
coord 


{ 


double cy) // Display center y-coord 


// Mark region centroids in evidence grid window 


double xd, yd; // Display coords 

char label[{ STRLEN] ; // Mark label (index) 
int mark_color; j 7 Mak. color 

at a 

For (i = 0; @ecnuntronc; ann) | 


xd = frontiers! i] .x — cx; 


Va = Lronrciers!1)-.y = ey; 
Marke Jcolon-= (lroncteustiicolor — 1) “5 DESPEAY OCOLOK., 
Ves Gout << Drawing Erontwer [" << a <<)") ian.” << 


color table[ mark color] 


ee <a" {0 (ac Mark color <<)" << endl; 


global window->set_color(color table[ mark_color] ); 

giopal. window->display_ circle (xd, yd, CENTROID MARK RADIUS); 

global window->display line(xd - CENTROID_ MARK _ RADIUS, Va, 
cit CENTROID MARK RADIUS, yd); 

global window->display line(xd, yd - CENTROID MARK RADIUS, 
xd, yd + CENTROID MARK RADIUS); 


SPrIneit (abel, "ta" 2); 
global _window->display text(xd + CENTROID MARK RADIUS * 2.0, yd, 


label); 


global window->set color ("black"); 


} 
global _ window->flush(); 
ji ‘out <<. endl; 


// for (i = 0; i < DISPLAY_COLORS; i++) { 


ZT 


3763 
3764 
5105 
3766 
510) 
3768 
2709 
a0 
S771 
Seeee 
3773 
3774 
37719 
3776 
ay07 
3778 
3719 
3780 
o751 
3782 
3783 
3784 
3785 
3786 
3787 
3788 
3789 
3790 
SSM 
3792 
373 
3794 
oS 
3796 
ao 7 
3798 
ooo 
3800 
3801 
3802 
3803 
3804 
3805 
3806 
3807 
3808 
3809 
3810 
3811 
3812 
3813 
3814 
3815 
3816 
3817 
3818 
3819 
3820 


// global _window->set_color(color table{ i] ); 
hd global windew=>display Jine (0, 1° =100 me l00G, a -100)F 
// } 


// global _window->set_ color ("black") ; 
// global _window->flush(); 

VOLd Agent: .display Sopot reqienucent roids (vod) 
// Mark region centroids in robot window 

// Display coords 


// Mark color 
// Color mode for draw command 


int. xa, yar 

int Marx color, 
int color mode; 
We, Pe 


refresh all(); 


fon (i =O < PUM eOme 1) 

xa. =] (int) frontiers! 2) <x; 

yd = (int) frontiers[ i] .y; 

Ty Maxrk color = (Lrontiers[ a)iicolor =tl te plore acorns 

es color mode = robot _color{ mark color] + 2; 

// Sour <<" oDrawiaGd frontier i. <i ea ce 
color table{ mark_color] 

doy —<— " {! “<< robot color mark Color =<-)) )mimedom ue. 
color mode <- |" 

fer << endl; 

Gelon mode=—- 


draw are(xd = (int) GENDROID MARK RADIUS, yao (ine) 
CENTROID MARK RADIUS, 


(int) (CENTROID MARK RADIUS * 2.0), 
(int) (CENTROID MARK RADIUS * 2.0), 
0, 3600, color mode); 


draw_line(xd - (int) CENTROID MARK RADIUS, yd, 
xd + (int) CENTROID MARK RADIUS, yd, color mode); 
draw line(xd, yd - (int) CENTROID MARK RADIUS, 
xd, yd + (int) CENTROID MARK RADIUS, color mode); 
} 
// cout << endl; 


// for (i = 0; i < DISPLAY_COLORS; i++) { 
if color mode = seseerecoleri 1) + 2; 

// draw_line(0, i * ~-100, 1000, i * ~100, color_mode); 
| | 


} 


// Cellvandex 
// Frontier index 


int agent::check. fronticetcell (ine xine 1, 
int front index) 
{ 


// Check whether cell (x, y) is part of frontier <front_index> 
if (frontiers[ front _index] .color == region map{ x][ y]) { 


return(l); 


} 
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3821 
3822 
3823 
3824 
3825 
3826 
3827 
3828 
3829 
3830 
3831 
3832 
3833 
3834 
3835 
3836 
3837 
3838 
3839 
3840 
384] 
3842 
3843 
3844 
3845 
3846 
3847 
3848 
3849 
3850 
3851 
3852 
3853 
3854 
3855 
3856 
3857 
3858 
3859 
3860 
3861 
3862 
3863 
3864 
3865 
3866 
3867 
3868 
3869 
3870 
3871 
3872 
3873 
3874 
3875 
3876 
3877 
3878 


else { 
return (0); 
} 
} 


[REREAD A EEE ES NAVIGATION FUNCTIONS LER EKER ERE AEH EA / 


void agent::corridor advance (void) 


{ 


J) Move forward at front corridor is clear 


i£ (wide corridor FWD] == 1) { 
// TEMP FIX for SCOUT comment out mv command below and change to 
SCout vim 


// mv (MV_VM, CORRIDOR SPEED WIDE, MV_IGNORE, 0, MV_IGNORE, 0); 
Cout << “In corrador advance wide rcorridor abour to veal Scout vm 4" 
<< CORRIDOR_SPEED WIDE << ", 0" << endl; // TEMP FIX for SCOUT 
Scout. ym (CORRIDOR, SPEED WIDE, 0); 9/7 TEMP FIX for scour 


} 
else if (corridor{ FWD] == 1) { 
// TEMP FIX for SCOUT comment out mv command below and change to 
scout _vm 
ve, mv (MV_VM, CORRIDOR_SPEED, MV_IGNORE, 0, MV_IGNORE, 0); 
Gout << ~ ln Corridor advance corridor about to call scout vm 4” 
<< CORRIDOR SPEED << ", 0" << endl; // TEMP FIX for SCOUT 
scout_vm(CORRIDOR_SPEED, 0); // TEMP FIX for SCOUT 
} 
else { 
// TEMP FIX for SCOUT comment out mv command below and change to 
scout_vm 
te mv (MV_VM, 0, MV_IGNORE, 0, MV_IGNORE, 0); 
eout << "In Corridor advance else about tO call Scout vm (0,0° << endl 
(/ STEMe Fix for SCOUT 
Seoue vm(0; 0); 7/ TEMP FIX for “Scour 
} 
} 


VOlGredGeni-. -9Odl COLE1CdOr ObleneliIne Gx, 2huegy} 
{ 


// Turn toward clear corridor closest to goal bearing 


double bearing; // Bearing from robot to goal 

double corridor index; // Index of selected corridor (-1 = none) 
double corridor bearing; // Bearing of selected corridor 

double cmd_bearing; // Bearing to face 

update (); 


bearing = atan2( (double) (qy —- r:y), (double) (gx — r.x)) * RADZDEG; 


Th Cout. <<" Coal ere oe ic ay ee eer ont. — ce 
r.“u << ie tt 

yy “CDE. yess. le eCcastancer— — 

// <<ohypor (| Gouble) (Gy --15.7)47 (couple) (ex) 95.) One 

Va << " : bearing =" << bearing << endl; 


GEECeCEBCOrriCors(); 
cOnridonm andexs—iselectecerridor | bearing); 
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33/9 
3880 
3881 
3882 
3883 
3884 
3885 
3886 
3887 
3888 
3889 
3890 
3891 
3892 
3893 
3894 
3895 
3896 
3897 
3898 
3899 
3900 
3901 
3902 
3903 
3904 
BS 
3906 
3907 
3908 
3909 
Sgn 
3911 
SoZ 
303 
3914 
Su 
3916 
3917 
3918 
SN, 
3920 
3921 
S922 
3923 
3924 
a025 
3926 
3o7 7) 
3928 
3929 
3930 
3951 
3932 
59355 
3934 
3955 
3936 


corridor bearing = 


angle wrap((double) (corridor index * SENSOR_SEP + r.theta) / 


LOO i; 
if ((corridor index == -1) [I 
(angle diff (bearing, corridor bearing) > CORRIDOR_MAX DEVIATION) ) 
{ 
cmd bearing = 
angle wrap((double) r.theta / 10.0 + 
rdrand(-GOAL_CORRIDOR_NOISE, GOAL CORRIDOR_NOISE)); 
} 
else { 
cmd bearing = 
angle wrap(corridor bearing + 
rdrand(-GOAL CORRIDOR NOISE, GOAL CORRIDOR _NOISE)); 
} 
Tf Cont << corridor index =" << Vcore don @ndex a= " corridor 
bearing = " 
fs << corridor bearing << " command bearing =" << 


end béaring«-<. endl, 


r.face angle fast( (int) OOH De. // TEMP Pix fom 


Scour 
} 


(ends bearing = 


void agent: : update nav_grid(void) 

{ 
// Update navigation grid based on global grid 
// grid_fine_to_coarse(global grid, nav_grid); 


Grid! copy Navagrad, global. grid); 


// grid _display(nav_window, nav_ grid); 


} 


int agent::path_plan(double wx, double wy, 
path &nav_path) 


// World coords of goal 
// Navigation path (optimized) 
{ 

// Plan path to goal location (return 1 if path found, 0 otherwise) 
path rev_ path; 
path unopt_path; 
path opt path; 
Int: Gx; OV, ozs 


// Reversed path 
// Unoptimized navigation path 
// Optimized navigation path 
// Grid coordinates of destination 
int ex rye ee 77 Gr id, Coordiiates, Of TODOE 
ihe, Ys // Cell index 
int search status; // Flag indicating whether path has been found 
world2grid(nav_grid, 


(double) r.x / 120.0, (double) roy 7 120ee 


CO, ekEx GLY, ab z)g 
worldZgrid(nav grid, wx / 120.0,;-wy 7 120.0, ©, Sox; “Soy, sqz)- 
cout << “Robot location = (" <<"“rex <4, <4 Poy ee eee 


i '? 
7 


<_ry <<") << endl- 
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B93 / 
3938 
3939 
3940 
3941 
3942 
3943 
3944 
3945 
3946 
3947 
3948 
3949 
3950 
Bo) | 
BOD 2 
B53 
3954 
By)) 
3956 
Bo) / 
3958 
Bo 59 
3960 
3961 
3962 
3963 
3964 
3965 
3966 
B20] 
3968 
3969 
3970 
5971 
Bd /2 
Bo 73 
3974 
8975 
3976 
BOTT 
3978 
Bo 79 
3980 
3981 
3982 
3983 
3984 
3985 
3986 
3987 
3988 
3989 
3990 
599 | 
3992 
5993 
3994 


} 


COuty 4-8 Goo WelocatioOne=at << wx << ", * << wy << ") 1" <<eo-3<-2, 
ee en | i eG. 


update nav_grid(); 


for (x = 907 x < NAV xX RES; xt) 
for (y = 0; y < NAVY RES; ytt) 4 
visit, x/[ yy] = 0; 
} 
} 


Search status = Lind sparn (rx, ©y, Gx, GY, fev pach); 


if “4SCOVeCn SEacuSs=—— SEARCH JFATL { 
cout << "No Pakhn found.” << endl; 
return(0); 


} 


reverse path(rev_path, unopt_path); 
GOuUE =< “UnOPEImized: —; 
PEINE Path unope path); 


optimize path(unopt_path, opt_path); 
COuUe == Optimizea: “7 
print path(opt_path); 


generate world path(opt_path, nav_path); 
Cour << "World path:"? 
Prine Vaeninavy pat): 


// grid_display(nav_window, nav_grid); 

// display _path(nav_path, OPT _PATH COLOR, nav_window) ; 
display path(nav_path, OPT PATH ECOLSR:, gilopa is window) ; 

// display path robot (nav path, ROBOT OPT PATH COLOR); 


meturn ( l)-: 


int agent::frontier path plan(double wx, double wy, // World coords of 
goal 


int front index, // Frontier index 
path &nav_path) // Navigation path 


// Plan path to goal location (return 1 if path found, 0 otherwise) 


Path, reyvipath; ) 4// Reversed parh 

path unopt_ path; // Unoptimized navigation path 

path opt path; // Optimized navigation path 

int gx, gy, gz; // Grid coordinates of destination 

INE@Ee S)ry, £27 8/7 © ord) COOrdinarcs Ore ronoE 

PEG, ay: // Cell index 

INE wSeaLrecho status, // Flag indicating whether path has been found 


WOLrOCGeIO4Maveguic,..  (Goubley: ssl 20 OU e(delbic) sr yea Os, 
OF sac cS EYP EZ) 


Wot ld2gGrd (av grid, o wx 2120-07 ewy 7/7 120 U0 eo, 1 aGy,. Gz); 


281 


3995 
3996 
3997 
3998 
B99 
4000 
4001 
4002 
4003 
4004 
4005 
4006 
4007 
4008 
4009 
4010 
4011 
4012 
4013 
4014 
4015 
4016 
4017 
4018 
4019 
4020 
4021 
4022 
4023 
4024 
4025 
4026 
4027 
4028 
4029 
4030 
4031 
4032 
4033 
4034 
4035 
4036 
4037 
4038 
4039 
4040 
4041 
4042 
4043 
4044 
4045 
4046 
4047 
4048 
4049 
4050 
4051 
4052 


cout << "Reber location = (<< "rox << 7G ee ee a ee 
ve 
<< ry << “]" << enal; 
cout << “Goal. location = (" << wx <<" << wy << 9" << G3) pee 


<< Gy <<. << “endl; 
update nav_grid(); 


for. (x = 0; % < NAV &% RES; x2) of 
for (y = 07; y < NAV_ YIKES? oyrt) 4 
visit{ x])[ y] = 0; 
} 
} 


Search status = frontier tind  pauh(rx, Ly, Gx, ay, Lrone unde 
rev_path); 
if ((search_status == SEARCH FAIL) || (search_status == 


SEARCH TIMEOUT)) { 


} 


void agent: 


{ 


Cour << “No path tound.” << endl; 
return(0Q); 


j 
reverse path(rev_path, unopt_path); 


eout << “Unoptimi zed; *; 
print path(unopt_path); 


optimize path(unopt path, opt path); 
cout <<- "Optimized: “; 
PEINe path tepel path); 


generate world path(opt_path, nav_path); 

e0un <= “World paren. > 

Print p2aLhinav path); 

// grid _display(nav_window, nav_grid); 

// display _path(nav_path, OPT PATH COLOR, nav_window) ; 
digplay path nay sath, ORT PATH _COLOR, global window) ; 
es display path robot (nav_path, ROBOT OPT PATH COLOR); 


return (2); 


>PEINT path(path p) 


// Print all cells on path 


Le aly 
cout << "path length =" << p.length << " : path = 
for (2 =07 a-< p.lenorn; ae) { 

cout << *(" << peakipy <<", " << ‘pivia) << 7) 


} 


cout << endl: 
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4053 
4054 
4055 
4056 
4057 
4058 
4059 
4060 
4061 
4062 
4063 
4064 
4065 
4066 
4067 
4068 
4069 
4070 
4071 
4072 
4073 
4074 
4075 
4076 
4077 
4078 
4079 
4080 
4081 
4082 
4083 
4084 
4085 

4086 
4087 
4088 
4089 
4090 
409] 
4092 
4093 
4094 
4095 
4096 
4097 
4098 
4099 
4100 
4101 
4102 
4103 
4104 
4105 
4106 
4107 
4108 
4109 
4110 


} 


void agent::display path(path p, // Path 
ehar *pcolor, // Path color 
window * win) // Window 


} 


// Draw path in window 
imte. 2; 
win->set_color(pcolor); 
fOr (i= OF 2 -< pp, length — 17, 177) { 
Win--display linet poxi a) 7) pov al] b-ball 
} 


Warn oer e SiC) ¢ 
Wino sel color (black |: 


void agent::display path robot (path p, iy Pach 


{ 


} 


Int BCoOror) jf Path coror 
// Draw path in robot window 
aed, 
fOr (t=O a Pe Lengen — lp igs) 4 


dvaw Slime (ossr 1] tecpey) tt, Pex tp. yi 1 pcolor mz 
} 


int agent::find path(int sx, int sy, // Start cell 
Mit Ge, ne Ay, // Goal cell 
path &p) // Path 


} 


/7 Band path: from (sx; sy) to (gx, gy) 

aimee ice ST // Neighbor cell index 
bath inte tpi; 

visit{ sx][ sy] = 1; 


while(closest_neighbor(sx, sy, gx, gy, nx, ny)) { 
if (search_cell(nx, ny, gx, gy, p) == SEARCH SUCCESS) { 
Va GOUL Fs PONV PATH (ec a eee 6 Coen a) rena: 
Patheadd(p, Sx,9sy); 
Perurn (| SEARCH SUCCESS); 
} 
} 


SeEUr Ot She Rey HALE); 


IMEPAgen= —fhontier tindupach(imerss tiniest, {7 Startecels 


Inia gy In Gmcy // Goal cell 
int front_index, // Frontier index 
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4111 
4112 
4113 
4114 
4115 
4116 
4117 
4118 
4119 
4120 
4121 
4122 
4123 
4124 
ANS 
4126 
4127 
4128 
4129 
4130 
4131 
4132 
4133 
4134 
4135 
4136 
4137 
4138 
4139 
4140 
4141 
4142 
4143 
4144 
4145 
4146 
4147 
4148 
4149 
4150 
4151 
4152 
4153 
4154 
4155 
4156 
4157 
4158 
4159 
4160 
4161 
4162 
4163 
4164 
4165 
4166 
4167 
4168 


path &p) // Path 
{ 
// Find path from (sx, sy) to 


<front_index> 


(QX%, GV) OF “any DoOint On-~tronties 


// Neighbor cell index 
// Cell search status 


ANG =nks, wy; 
int status; 


path init (p); 
visit{ sx][ sy] = 1; 


while(closest neighbor(sx, sy, 9X, gy-r nx, ny)) { 


cell count. — 0; 
status = frontier search cell(nx, ny, gx, gy, front _index, p); 
if (status == SEARCH SUCCESS) { 
77 cout << "[ ON PATH (" << x << ", " << y << ") ]" <2 ene 


Path add(p, sx, sy); 
return(SEARCH SUCCESS) ; 
} 
if (status == SEARCH TIMEOUT) { 
return (SEARCH TIMEOUT) ; 
} 
} 


return (SEARCH FAIL); 
} 


// Search cell 
// Goal cell 
/7 Patch 


int agent::search cell (int x, int y, 
Tater, Lhe Oy, 
path &p) 
// Search cell (x,y) and return search status 


// Search status 
// Neighbor cell index 


Int “status; 
pie. one ye 


Lf Jyus2 ch xy) 


eout << "search ‘cell: ‘Errors: revisited cell {" << % << "502 6 eee 
tt ye 
<< endl; 
exit (-1); 


} 

visit[ xJ[ y) = 1; 

ili @OGR =40" Seavehime Cl sce 6a pe eee ee ee 

if ((x < 0) || (x >= NAV_X RES) II (y < 0) [I 
7 cout << "Out of bounds." << endl; 
Lecurn (SEARCH FArh) 7 

} 


(y >= NAV_Y_RES)) { 


if ((x == gx) && (y == gy)) { 
// Colic, =< "(= COnL =a 3 <=. 
path _add(p, x, y); 
return (SEARCH SUCCESS) ; 

} 


UN aT je | ee ence, 
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4169 


4170 if ((nav_grid.mapn{ grid2index(nav_grid, x, y, 0)] >= 0) || 

4171 (!check clear(x, y))) { 

4172 ie Gout =< "> BLOCKED =<" << endl, 

4173 return(SEARCH FAIL); 

4174 } 

4175 

4176 //- Gout << "(4 Searching adjacent 3)" << endl; 

4177 

4178 while(closest_neighbor(x, y, 9x, gy, nx, ny)) { 

4179 if (search _cell(nx, ny, gx, gy, Pp) == SEARCH SUCCESS) { 

4180 // eo <<)" VON PATH (ae oe ey ce endl: 

4181 path_addi(p, xX, Y); 

4182 return (SEARCH SUCCESS) ; 

4183 } 

4184 

4185 

4186 return (SEARCH FAIL); 

4187} 

4188 

4189 UE agent. (front rver search célliinte x7 1nt y, // Search cell 

4190 Ime Gx, ini cy. // Goal cell 

4191 int front_index, // Frontier index 

4192 path &p) // Path 

4193 { 

4194 // Search cell (x,y) while navigating to frontier and return search 

4195 —s status 

4196 

4197 int child_status; // Search status for child cell 

4198 intense, ny; // Neighbor cell index 

4199 

4200 cell count++; 

4201 if (cell count % 100 == 0) { 

03 Eon << "Séaqrenang "<< céll cotnt <<" célls,.,"" << endl; 
} 

4204 if (cell_count > SEARCH MAX CELLS) { 

4205 cell count = 0; 

4206 return (SEARCH TIMEOUT) ; 

4207 7 

4208 

4209 Le Cvalsath spp yl} | 

4210 COUU——e PrOntier Searenyveeli:, Beron. revisited Gell) aoe 

Bell 8 << y << ")" = 

4212 << endl; 

4213 exe (1). 

4214 } 

4215 Viste lt yi ls 

4216 

4217 JP Secure <<) Searching (neo cco ee 

4218 

4219 if ((x < 0) || (x >= NAV_X RES) || (y < 0) [fl (y >= NAV_Y RES)) { 

4220 // Goute<< "Out of bounces. << endl _—— 

4221 

4222 return(SEARCH FAIL); 

4223 } = 

4224 

4225 

4226 // if ((x == gx) && (y == gy)) { 
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4227 
4228 
4229 
4230 
4231 
4232 
4233 
4234 
4235 
4236 
4237 
4238 
4239 
4240 
424] 
4242 
4243 
4244 
4245 
4246 
4247 
4248 
4249 
4250 
4251 
4252 
4253 
4254 
4255 
4256 
4257 
4258 
4259 
4260 
4261 
4262 
4263 
4264 
4265 
4266 
4267 
4268 
4269 
4270 
4271 
4272 
4273 
4274 
4275 
4276 
4277 
4278 
4279 
4280 
4281 
4282 
4283 
4284 


DE 1 SO ey = oy 
(Ghéeck -frontrter arrival (x, y,. Pronk: index) ya, 
is Gout << "f= GOAL (" << xoe< "Soe eee orencdi, 


path_add(p, x, y); 
return (SEARCH SUCCESS); 
} 


wi (inay sora. mapm, gridZindex(nav grid, ox, y, 20) = 0). i 
(reneck cleariz, y) 54 
ee Gout << "> BLOCKED <<" §<4< end; 
ECLUEN{ SEARCH PALL), 
} 
jj/- eout << "(( Searching adjacent ))" << -endiz 
Whi letebosest mercgnber(x, vy. Gx, Gy, Mx, my) ) 4 
child status = frontier search cell (nx, ny, 9x, gy, front_index7aee 
@f (chile tstatus—— sEakeGH SUCCESS) { 
Vp cout << "[ ON PATH (" << x << ", " << y << ") |” << ence 


path add(p, x, y); 
return (SEARCH SUCCESS) ; 
} 
if (child_status == SEARCH TIMEOUT) { 
Beturn( SEARCH  TIMECUT); 
} 
} 


return (SEARCH FAIL); 
} 


int agent::closest neighbor(int x, int y, // Current cell index 


ine Gs, ne oy, // Goal cell index 
int &nx, int &ny) // Next eell index 
{ 
// Find index of (unvisited) neighbor closest to goal 


// Minimum distance from neighbor to goal 
// Distance from neighbor to goal 

// 1 if unvisited neighbor exists, 0 otherwise 
// Adjacent cell offset 
// Adjacent cell index 


double min_dist; 
double dist; 

int: found = 0; 
int “ax, dy. 

int ax, ay; 


min Gist = (double) (MAc PAT hs LENGIH, 


for (aoe = —1l, dx =, saxty 
for (ay = -—ly dy <= 27 7cy-7) “{ 
ax = x + dx; 
ay = y + dy; 


if ((ax >= 0) && 
NAV_Y RES)) { 
if (visit[ ax][ ay] == 0) { 
dist = hypot(gx = ax, gy = ay); 
L£ (Gist < mangoes) | 


(ax < NAV _X RES) && (ay >= 0) && (ay < 


min dist = dist; 
NX = ax; 
ny = ay; 
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4285 
4286 
4287 
4288 
4289 
4290 
4291 
4292 
4293 
4294 
4295 
4296 
4297 
4298 
4299 
4300 
4301 
4302 
4303 
4304 
4305 
4306 
4307 
4308 
4309 
4310 
4311 
4312 
4313 
4314 
4315 
4316 
4317 
4318 
4319 
4320 
4321 
4322 
4323 
4324 
4325 
4326 
4327 
4328 
4329 
4330 
4331 
4332 
4333 
4334 
4335 
4336 
4337 
4338 
4339 
4340 
434] 


} 


recurn found) > 


void agent::reverse path(path old path, // Inatial path 


{ 


} 


path &new path) // Reversed path 
// Reverse order of steps on path 
aes 


path init (new_path); 
for (i = 90; 26 Old path. length, 1+5.) 4 
Path oadd (new path, old path x fold pathlength = 1b) = ai, 
old pathey. (old path tence = 1) 1) 77 
} 


void agent::optimize path(path old path, // Initial path 


{ 


path &new_ path) // Optimized path 
// Optimize path by jumping between adjacent path cells 
int marker = 0; // Point along old path 
int jump marker; // Point to jump to on new path 


iotejumpe clage // (Ser to J af path jumps 


path_init (new_path) ; 
path add(new_ path, old_path.x[ 0), old path.y 0] ); 


jf) VCOur <q “Startingrar. (| (<< 9new pach.x( GU] << 972" <= new parn. vied) 
if <i Ol) 0S" << ena; 


woale(manker =< Old path. lenge = 4 
jump flag = 0; 


i cout << "Trying to jump from (" << new_path.x{ new path.length 
fos 1) <co" ; " 
Lf << new path.y{ new path.length - 1) << ") [" << 


Mewnpath«lengtn. = 1 


voy. << "Rey tee marker <<1" >" o<<. endl: 
for Aj7UMp Markery—=~old path. length y- "4; 
(jump marker > marker) && !jump flag; 


Una Mate Keira 3) at 

// Coutle<<1 Checking 4 <<yold: path. <a) Umpeman ken)" < 

// << Old lpatheyl jump Marker), <<)" j9<"0 << jump marker.) =" 
<< endl; 


if ((old_path.x{ Jump_marker] ~- old path.x{[ marker] >= -1) && 
(old_path.x{ jump_marker] - old _path.x[ marker] <= 1) && 
(old path.y{ jump marker] - old path.y[marker] >= -1) && 
(old _path.y Jump _ marker] - old path.y{ marker) <= 1)) { 
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4342 
4343 
4344 
4345 
4346 
4347 
4348 
4349 
4350 
4351 
4352 
4353 
4354 
4355 
4356 
4357 
4358 
4359 
4360 
4361 
4362 
4363 
4364 
4365 
4366 
4367 
4368 
4369 
4370 
437] 
4372 
4373 
4374 
4375 
4376 
4377 
4378 
4379 
4380 
438] 
4382 
4383 
4384 
4385 
4386 
4387 
4388 
4389 
4390 
439] 
2 
4393 
4394 
4395 
4396 
4397 
4398 
4399 


path_add(new_path, old _path.x{ jump marker], 
old path.y{ Jump_marker] ); 


// cout << "Jumping from (" << new_path.x[ new path.length - 2] 
<< — 1" 

ras <“qonew Pathe yi new path. length =. 27) << eto 

Ff << new_path.x{ new _path.length - 1) <<", " 

a << new path. yi new path. length — 1) <<")? endl 


Marker = jump marker; 

jump flag = 1; 

} 

} 
} 
} 
void agent::generate world path(path grid path, // Path in nav 
grid 
path &world path) // Path in world coords 


{ 


// Convert path in grid cell coords to world coords 


double wx, wy; // World coords 


double xscale, yscale, zscale; // Cell dimensions (tenths of 
inches) 

int xsize, ysize, zsize; // Grid dimensions (# cells) 

atte 5 

xSize = nav_grid.msize[ 0]; 

ysize = nav_grid.msizef{ 1) ; 

zsize = nav_grid.msizef 2] ; 

xscale = (nav_grid.himvy 0] -— nav grid.lomy 0] } * 120.0 7 ideubie 
xSize; 

yscale = (nav_grid.himv{ 1] - nav_grid.lomv{1])) * 120.0 / (double) 
ysize; 

zscale = (nav_grid.himv{ 2] - nav_grid.lomvj 2] ) * 120.0 / (double) 
zsize; 


path init (world path); 


Poe 
wx 


O07 bs grid path length, ata) 3 

(((aouble) -guadipath.s| 1) +025), = xscale 
+ Nay geid. tomy 0] ~ 220-20); 
wy = (((double) grid path.y{i] + 0.5) * yscale 
+ Nav grid.lomy 2), © -120.0); 


path_add(world path, (int) wx, (int) wy); 


} 
} 


void agent::path_init(path &p) // Path 
{ 

// Initialize path 

p.length = QO; 
void agent::path_add(path &p, // Path 
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4400 
4401 
4402 
4403 
4404 
4405 
4406 
4407 
4408 
4409 
4410 
4411 
4412 
4413 
4414 
4415 
4416 
4417 
4418 
4419 
4420 
4421 
4422 
4423 
4424 
4425 
4426 
4427 
4428 
4429 
4430 
4431 
4432 
4433 
4434 
4435 
4436 
4437 
4438 
4439 
4440 
444] 
4442 
4443 
aad 
4445 
4446 
4447 
4448 
4449 
4450 
4451 
4452 
4453 
4454 
4455 
4456 
4457 


{ 


// BRdd point to 
Le 


endl; 


} 

Oe 
me 
oe 


} 


ioe sagen; seneck clear(int x, int 


{ 


ico eel | // Point to add to path 


path 


(p.length == MAX PATH LENGTH) { 
Cour, <=. pach add; Toc many steps (> << MAX PATH) LENGTH <<) ji <- 
exit —i); 
Hi Oe Lengtn = x7 
yi p-length]) = y; 
Lengiune +; 


y) 


// Check to see whether region around point is free of known 


obstacles 

Mc aODs COUnt =O, jf Obstacle counter 

Meme Yi // Grid indices 

ante x1, xp yilz. yh, zl, zh; 7/7 Grid coords of robot—-sized box around 
POLNe 

double wx, wy, WZ; // World coordinates of point 

double wxl, wxh, wyl, wyh; // World coords of robot-sized box around 
Delnt 

// int xsize, ysize; // Grid dimensions (# cells) 

// double xscale, yscale; // Cell dimensions (tenths of inches) 


// 


G@rid2worldimav rid, x; y, 


wxl 
wxh 
wyl 
wyh 


world2grid(nav_grid, wxl, 
world2grid(nav_grid, wxh, 


Tah 
ae 


// 
// 


for 
LOL 


double xd, yd; 


= WX 
= WX 
= wy 
= wy 


xsize 
ysize 


xscal 
yscal 


(xl = 


(yi 
ia 
Toh, 


// 
ey 
Tf 


Te (May 191 Mao gr LdZindex (naveguid esi 2,0} 


a 
+ 


e 
€ 


x 


// Display coords 
O, &wx, &wy, &wZ); 
ROBOT PASSAGE RADIUS; 
ROBOT PASSAGE RADIUS; 
ROBOT PASSAGE RADIUS; 
ROBOT PASSAGE RADIUS; 
wyl, WZ, 7&xl, &£Y¥1,) &2Z1); 
wy, wz, &xh, é&yh, &zh)i; 
NAV_X_RES; 
NAV_Y RES; 
= (NAV_X MAX - NAV X MIN) * 120.0 / (double) xsize; 
= (NAV_Y MAX - NAV Y MIN) * 120.0 / (double) ysize; 
Le ee Ki xa Ee 
Vile ie = by oy ae 
XC) = i clou be eit eee SGare + GROBA Le a Mili 312020; 
yar= (double) (yiet 0.5) = yseate + Clop le Mine 12050; 


global wandow-7 ser (color gold’); 
glebal window--display point (xd, yd); 
gicbal window--set colon” black’ ); 


ZO 


289 


4458 
4459 
4460 
4461 
4462 
4463 
4464 
4465 
4466 
4467 
4468 
4469 
4470 
447) 
4472 
4473 
4474 
4475 
4476 
4477 
4478 
4479 
4480 
4481 
4482 
4483 
4484 
4485 
4486 
4487 
4488 
4489 
4490 
4491 
4492 
4493 
4494 
4495 
4496 
4497 
4498 
4499 
4500 
4501 
4502 
4503 
4504 
4505 
4506 
4507 
4508 
4509 
4510 
4511 
4512 
4513 
4514 
4515 


} 


ie global _window->set_ color ("red") ; 
if global _window->display circle(xd, yd, xscale); 
a global_window->set_ color ("black"); 


@bS Counet+, 
} 
} 
j 


if (obs count > MAX OBS COUNT) { 
return ¢)- 

} 

else { 
~eturn (1) 


} 


Ime agente: check frontier arrival (int &, “int yy, Int. Crone nderd 


{ 


// Check to see whether region around point overlaps frontier 


Tits, // Grid indices 
int xl, xh, yl, yh, zl, zh; // Grid coords of robot-sized box around 
poing 
double wx, wy, WZ; // World coordinates (or point 
double wxl, wxh, wyl, wyh; // World coords of robot-sized box around 
PONG 
grid2world(nav_grid, x, y, 0, &wx, &wy, &WZ); 
wxl = wx - ROBOT PASSAGE RADIUS; 
wxh = wx + ROBOT PASSAGE RADIUS; 
wyl = wy - ROBOT PASSAGE RADIUS; 
wyh = wy + ROBOT PASSAGE RADIUS; 
world2grid(nav grid, wxl, wyl, wz, &xl, “4yl, ‘azi); 
world2grid(nav_grid, wxh, wyh, wz, &xh, &yh, &zh); 
Tor (Sil = x%1> xi <= xhe xit+) { 
fOr Ay ieee le ye ye 
if (check frontier cell(xi, yi, front index)) { 
return(l); 
} 
} 
} 
revirn(G). 


} 


double agent::closest waypoint (path p, 


// Path 


it ox elt ays // Current position {i717 


inch) 


// Index of current waypoint 
// Index of closest waypoint 


int index, 
int &close index) 


// Finds waypoint furthest on path within destination tolerance, or 
// waypoint on path <p> closest to (x, y), returning the distance 


(inches) 


// to that point, and the waypoint's index in <index> 
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4516 
4517 
4518 
4519 
4520 
4521 
4522 
4523 
4524 
4525 
4526 
4527 
4528 
4529 
4530 
4531 
4532 
4533 
4534 
4535 
4536 
4537 
4538 
4539 
4540 
454] 
4542 
4543 
4544 
4545 
4546 
4547 
4548 
4549 
4550 
4551 
4552 
4553 
4554 
4555 
4556 
4557 
4558 
4559 
4560 
4561 
4562 
4563 
4564 
4565 
4566 
4567 
4568 
4569 
4570 
457] 
4572 
4573 


double dist; // Distance to waypoint 

double min_dist = MAX_DIST; // Minimum distance to waypoint 

int last waypoint; // Last waypoint to check 

Lit ae; 

// COul << "CuUrrene position — (aoc, << 8 oe << yee |) es ena: 
36 ((andex =< 0) Ti (index 2—= p.lengih)) { 


cout << "closest waypoint: index <" << index << "> out of range 


<< length << ~~) 'ox< endl: 
exit (-1); 
} 


// Set lookahead window for checking waypoints 


last waypoint = index + WAYPOINT WINDOW; 
if (last waypoint > p.length - 1) { 

fast Waypoint. = p.tengtn — pi, 
} 


// Search for closest waypoint 


for (i = last waypoint; 1 >= index; i--) { 
dist = hypot((double) (p.xl i) - x), (double) (p.yf i) - y)) / 10.0; 
Vi SOUL <= “distance to waypoint [<< 1 <= "|" =< pox | 
Vf. <<be yl i) 64 3) = << cast << endl: 


Pier (arse amin asi) sy 
MLNed1Ste=sai se, 
close index = i; 


uf (Min dist <= LOCAL NAV TOLERANCE) { 
Gout << "ir VARRIVED =|". << enc! : 
return(min dist); 
} 
} 
} 


/f cout << “closest waypoint [" << close index << "] (" << 


p.x{ close_index] 


ae ai,  SSupe yl Closevindex| << 9" )i sidist. =" so iminwedcm 


endl; 


return(min dist); 


[Ok ke ee Fe tee te ke ee ke eee ee CORRIDOR FUNCTIONS Kk KKK KKK KKK KKK KKK KKK J 


VOld "agen: -Gdetcect “cor riders (youd) 


// Detect freespace cooridors in vicinity of robot 


Abas 
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4574 
4575 
4576 
4577 
4578 
4579 
4580 
4581 
4582 
4583 
4584 
4585 
4586 
4587 
4588 
4589 
4590 
4591 
4592 
4593 
4594 
4595 
4596 
4597 
4598 
4599 
4600 
4601 
4602 
4603 
4604 
4605 
4606 
4607 
4608 
4609 
4610 
4611 
4612 
4613 
4614 
4615 
4616 
4617 
4618 
4619 
4620 
4621 
4622 
4623 
4624 
4625 
4626 
4627 
4628 
4629 
4630 
4631 


update (); 


for (i = 0; i < NUM_RANGE; i++) { 
corridor, i) = check corrider(1; CORRIDOR PWDURANGE, 
CORRIDOR SIDE CLEARANCE) ; 
wide corridor[{ i] = check_corridor(i, CORRIDOR WIDE FWD RANGE, 
CORRIDOR WIDE SIDE CLEARANCE) ; 
} 


7?) worsplay “corradons(); 
// global window->flush(); 


jf Gesplay COrELdors(), 


int: agent; :check corridor(int cemger, // Index of sensor in center 
Sof corrider 
int fwd range, 


Ime stde eClear) 


// Required forward space 
// Required side space 


// Check whether a corridor exists centered around sensor <center> 
// Return 1 if true, O otherwise 


// Sensor index 
// Birste Sensor Eo be checked 
// Last sensor to be checked 


int sensor; 
iNet Stare, 
int end; 
// cout << "Checking corridor [" << center << "]" << endl; // TEMP Maa 
for SolLOUT 


start = wrap(center - CORRIDOR_SPAN, 0, NUM_RANGE - 1); 
end = wrap(center + CORRIDOR _SPAN, 0, NUM RANGE - 1); 


Vf (start -<. end) -{ 
for (sensor = start; sensor <= end; sensor+t) { 
if (!corridor check sensor(center, sensor, fwd_range, side ¢leaus 
{ 
J}/ COouk << "Corridor ("" << center << "] 
TEMP FIX for SCOUT 
return (0) 
} 
} 


Jf Gout << "Corzmador |” << center <<<") 
TEMP PIX tor Seceur 
return(1); 


} 


is >> BLOCKED <<." << endl] - ir, 


is.[[ OPEN J] }:."” << ene Te 


for (sensor = Start; sensor < NUM RANGE; sensor++) { 
if (!corridor check sensor(center, sensor, fwd range, side cilcaremy 
// cout << "Corridor (" << center << "] is >> BLOCKED <<." << encditaee 
TEMP Fix for Scour 


return(Q); 
} 
} 
for (sensor = 0; sensor <= end; sensort+t) { 
if (!corridor check sensor(center, sensor, fwd range, side clear)) { 
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4632 
4633 
4634 
4635 
4636 
4637 
4638 
4639 
4640 
464] 
4642 
4643 
4644 
4645 
4646 
4647 
4648 
4649 
4650 
465] 
4652 
4653 
4654 
4655 
4656 
4657 
4658 
4659 
4660 
4661 
4662 
4663 
4664 
4665 
4666 
4667 
4668 
4669 
4670 
4671 
4672 
4673 
4674 
4675 
4676 
4677 
4678 
4679 
4680 
4681 
4682 
4683 
4684 
4685 
4686 
4687 
4688 
4689 


J// cout << “Corridor |") << icénter << "]) ts >> BLOCKED << ."8<< ‘endl: 7/ 
TEMP’ Bile Lor SCOUT 
return (0); 
} 
} 
Jy. cout. <= “Corridor [| << center << 7], 1s [— .eEN |] ) .” << endl: (77 
TEMP FIX for SCOUT 
return (1); 
} 
Pit echt. -cOrridor Check Sensor(ine center, // Center sensor 
index 
int sensor, // Sensor index 
int fwd range, // Required fwd space 


int side clear) // Required side space 


{ 


// Check whether <sensor> is clear for corridor <center> 


const double bot width = ROBOT RADIUS * 12.0; // Robot width (inches) 


const double sens _ sep = SENSOR SEP * 0.1; // Sensor separation 
(degrees) 
double angle; // Sensor angle (degrees) 
double center angle; // Angle of center sensor (degrees) 
double theta; // Angle (degrees) between sensor and 
// perpendicular to center angle 
double thresh; // Minimum clear distance (inches) 


Center angle *=-an¢gle Wrap ( (double) te. tmeta = 0.) 
7 (OUR le) cegeem “sens Sep), 
// TEMP FIX for SCOUT - changed r.turret to r.theta on previous line 
// cout << "center [" << center << "] : center angle =" << center angle 
<< endl; // TEMP FIX for SCOUT 


angle — ang leswiap | (doubrejer checas 0. 1Fso (double) sensors 
sens sep); // TEMP FIX for SCOUT r.turret to r.theta 
theta = 90.0 - angle diff(center_ angle, angle); 


if (center == sensor) { 
thresh = fwd_range; 
} 
ese 
thresh = (bot_width + side clear) / cos(theta * DEG2RAD) 
SE bOEoWwTctihy 
if {thresh se) Ewd range) ™{ 
thresh = fwd range; 
} 
} 


// cout << "sensor [" << sensor << "] : sensor angle =" << angle 
ef <a" “9 thetar= "<< theta .<< “= thresh — " <—) thresh 
// << " :; range =" << r.range[ sensor]; // TEMP FIX for SCOUT 


if (r.range{ sensor] < thresh) { 
Ji ~COUkEs<) "=~ BLOCKED"? << endl.“ 7/7 -TEME Fix for-SCour 
return (0) 


} 


else { 
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4690 
4691 
4692 
4693 
4694 
4695 
4696 
4697 
4698 
4699 
4700 
4701 
4702 
4703 
4704 
4705 
4706 
4707 
4708 
4709 
4710 
4711 
4712 
4713 
4714 
4715 
4716 
4717 
4718 
4719 
4720 
4721 
4722 
4723 
4724 
4725 
4726 
4727 
4728 
4729 
4730 
4731 
4732 
4733 
4734 
4735 
4736 
4737 
4738 
4739 
4740 
4741 
4742 
4743 
4744 
4745 
4746 
4747 


i / $eOute <= "| -CLEAR | =< endl: J// TEMP FIX for Seour 
recwoen (a) 


} 
} 


void agent:: 


{ 


display corridors (void) 
// Display corridors in robot window 
in 17 
// xvefresh_all(); 
for (1/= 0; 2 <“NUMEEANGE, i172) 4 
if (wide _corridor[ 1] == 1) { 


display_ corridor (global_window, i, CORRIDOR_WIDE FWD RANGE, 
CORRIDOR _ WIDE SIDE CLEARANCE, CORRIDOR _ WIDE , CORR 


// display _ corridor _robot (i, CORRIDOR WIDE _ FWD _ RANGE, 
ee CORRIDOR WIDE SIDE | CLEARANCE, 
Ti CORRIDOR WIDE COLOR ROBOT) ; 

} 

else if (corridor[ iJ == 1) { 


display corridor (global window, i, CORRIDOR _FWD_RANGE, 
CORRIDOR SIDE CLEARANCE, CORRIDOR COLOR); 

TL? display _ corridor Probert CORRIDOR FWD RANGE, 
Vek CORRIDOR_SIDE | CLEARANCE, 

CORRIDOR COLOR ROBOT); 

} 
} 
} 


:display corridor (window *win, // Window 
int “center, // Center sensor ide 
int fwd_range, // Required forward space 
int side clear, // Required side space 
Ghar * color) // *ceruiderecelo: 


void agent: 


// Display corridor boundaries centered around sensor <center> 


7 / KOCOE widen ii/ 10> ineh) 
const double bot_width = ROBOT RADIUS * 120.0; 


// Length of forward axis (1/10 inch) 

// Distance to either side of robot (1/10 inch) 
// Corridor angle (degrees) 
// Corner coords (1/10 inem 
// Offset for forward end 


double fwd dist; 
double side dist; 
double angle; 

Gouble 2) vl x2, v2, 
double fwd_x, fwd_y; 


Ko Vo; x4, 94; 


fwd dist = bot width + 
SiGe saise = bot widens 


(double) 
(double) 


fucderanges* 10.07 
side vclear ~*~ j020; 


// SCOUT THESIS CHANGE - changed r.turret to r.theta in line below 
angle = angle wrap((double) r.theta * 0.1 


+ (double) Weenmter = -SENSCR SEP) 202), 


xl = ©.x + Sidevdist * cos( (angle + 90.0) * DEGZRAD), 
Vl ="2.y + ‘side dise* san( (angle + 90.0) + DEGZRAD).. 
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4748 
4749 
4750 
4751 
4752 
4753 
4754 
4755 
4756 
4757 
4758 
4759 
4760 
4761 
4762 
4763 
4764 
4765 
4766 
4767 
4768 
4769 
4770 
477) 
4772 
4773 
4774 
4775 
4776 
4777 
4778 
4779 
4780 
4781 
4782 
4783 
4784 
4785 
4786 
4787 
4788 
4789 
4790 
4791 
4792 
4793 
4794 
4795 
4796 
4797 
4798 
4799 
4800 
4801 
4802 
4803 
4804 
4805 


} 


vold agene:. display Corridor Tobec {ine center, 


{ 


X20 = ax tocideéedist *secos( (angle = 90.0) * DEGZRAD); 
V2 ser y + SiGe Gist. = sini (angle = -90.0).~ DECZRAD), 
fwd x = fwd dist * cos{angle * DEGZRAD); 

fwd _y = fwd_dist * sin(angle * DEG2RAD); 

x3 = xl + fwd_x; 

y3 = yl + fwd_y; 

x4 = x2 + fwd_x; 

y4 = y2 + fwd_y; 


win->set_color(color); 


win->display line(xl, yl, x2, y2); 
win->display line(x2, y2, x4, y4); 
win->display line(x4, y4, x3, y3); 
Win->Gasplay lime (x3, ys; x1, yii- 


win->set_ color ("black"); 


// Center sensor index 
// Required forward space 
// Required side space 

/ 7 “Corridor color 


int fwd range, 
ine. side clear, 
ine Color} 


// Display corridor boundaries centered around sensor <center> in 


EFODOt window 


Ji Robot width. C1710. anen) 


e0nst double bot widen = ROBOT Ree EUS = 120.07 

double fwd dist; // Length of forward axis (1/10 inch) 

double side dist; // Distance to either side of robot (1/10 inch) 
double angle; // Corridor angle (degrees) 
dacuble x1, vile #2; v2; %S, Vs, £45 34> 7/7 Corneracoordse a), FO ginen) 
double fwd_x, fwd_y; // Offset for forward end 


twa dist = bot widen + (double) iwdieange ~ —F0.0; 
side dist = bot_width + (double) side clear * 10.0; 


// SCOUT THESIS CHANGE - changed r.turret to r.theta in line below 


angle = angle wrap((double) r.theta * 0.1 

7 (double) y (center -* SENCOR TSE) 20). 
MENS rx 4 Side.disee cos(langles+ 190.0). WDEGZRAD); 
yiX— ory te sidevdist.* sini (angle + 9070) \DEGZRAD; 
2 —eh. kota SiGerai St 2 6Cos (angle 7 90.0) (= PECZRAby: 
y2 =r.y + side dist * sin((angle - 90.0) * DEG2RAD); 
fwd x = fwd dist * cos(angle * DEG2RAD) ; 
fwd y = fwd _ dist * sin(angle * DEG2RAD); 
x3 = xl + fwd x; 
y3 = yl + fwd _y; 
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4806 
4807 
4808 
4809 
4810 
4811 
4812 
4313 
4814 
4815 
4816 
4817 
4818 
4819 
4820 
4821 
4822 
4823 
4824 
4825 
4826 
4827 
4828 
4829 
4830 
4831 
4832 
4833 
4834 
4835 
4836 
4837 
4838 
4839 
4840 
4841 
4842 
4843 
4844 
4845 
4846 
4847 
4848 
4849 
4850 
4851 
4852 
4853 
4854 
4855 
4856 
4857 
4858 
4859 
4860 
4861 
4862 


x4 = x2 + fwd_x; 
y4 = y2 + fwd_y; 
draw lanettint). 2s (int) “yl, (ane) 22, 
draws lene ine) 2 (int) 2, Vie) xs, 
draw line((int) x4, (int) y4, (int) x3, 
draw dineil({int) x3, (int) ya, (ant) x1, 


} 


int agent::select corridor (double heading) 


{ 


(Int) 
(ane) 
(ant) 
(int) 


color 
eo hon 
color 
Color 


y2, 
y4, 
y3, 
yl, 


t+ +++ 
NNMNN 


"=e 


// Heading (degrees) 


// Select corridor nearest to specified heading 


Conse Gouble sens Sep — 
(degrees) 

double angle; 

double dtheta; 

double min Genera — 


SENSOR_SEP * 


56020; 


int select = -l; 
inc. te 
heading = angle wrap (heading) ; 
for {1° = 0; 4. <— NUMURANGE, i++) { 
Lf (corridor, ij == 4 
angle — angle wrap (( double) © tneta = 7021 


+ (double) i * 


Oealy 


// Sensor angle 
// Angle/heading deviation 

// Minimum angle deviation 
// Index of selected corridor 


Seems 35ep) 7 


// Sensor separation 


// SCOUT THESIS CHANGE - use r.theta vice r.turret in line above 
// TEMP FIX for SCOUT - lets try some numbers checking below 
// cout << "About to call angle diff with heading= " << heading 


fy << "and angle = 

GEheta = angle wditt (heading, 
// cout << "dtheta = 
// TEMP FIX for. SCOUT 
// cout << "min dtheta = 
TEMP FIX for SCOUT 


Lf Wdehete < min Genera) { 


min dtheta = dtheta; 
select = i; 
} 
} 
} 
if (select == -1) { 


Cout. <=" No Open corriders.” << endl; 
return(select); 

} 
//SCOUT THESIS CHANGE - changed r.turret 
cout << "desired heading =" << heading 
<< select << "] corridor angle = 
ae 
Sens Sep) 
<< 


deviation = ™ << min dtheta 


if (wide corridor{ select] == 1) { 
ca display corridor (global window, 


CORRIDOR WIDE FWD RANGE, 
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angle wrap((double) r.theta * 0. 


" << angle << endl; 
angle) ; 
angle dift(heading,angle) 


ema eNews 


// TEMP FIX for SCOUuUn 
" << dtheta << ena 


=" << 31 << endl; vo 


to r.theta 3 lines down 


<< selected corridor [" 
1 + (double) select * 

<< endl; 

select, 


4863 
4864 
4865 
4866 
4867 
4868 
4869 
4870 
487] 
4872 
4873 
4874 
4875 
4876 
4877 
4878 
4879 
4880 
4881 
4882 
4883 
4884 
4885 
4886 
4887 
4888 
4889 
4890 
4891 
4892 
4893 
4894 
4895 
4896 
4897 
4898 
4899 
4900 
4901 
4902 
4903 
4904 
4905 
4906 
4907 
4908 
4909 
4910 
4911 
4912 
4913 
4914 
4915 
4916 
4917 
4918 
4919 
4920 


i CORRIDOR _WIDE_SIDE_ CLEARANCE, 
CORRIDOR_SELECT WIDE COLOR) ; 
ve display_ corridor robot (select, CORRIDOR WIDE FWD RANGE, 
Ji CORRIDOR WIDE SIDE _CLEARANCE, 
yy CORRIDOR SELECT WIDE COLOR ROBOT); 
} 
else { 
ea display corridor (global _ window, select, CORRIDOR_FWD_RANGE, 
Ly CORRIDOR _ SIDE CLEARANCE, CORRIDOR _ SELECT _COLOR) ; 
yy display corridor _robot (select, CORRIDOR _FWD RANGE, 
CORRIDOR_SIDE_CLEARANCE, 
hi CORRIDOR_SELECT COLOR ROBOT) ; 


} 


return(select); 


} 


ere ee ee KIN TERPACE. 10 CONTINUOUS TOCA ZATION =** 3 te 22a] 


void agent: :connect_cl (void) 


{ 


J// Tnictialize Communications With Continuous localization 
char comm_mach{ STRLEN] ; 
cout << "Enter continuous localization host ==> 


ein: >> comm_mach; 
Can.ceer () 


eo, 
f 


// connect_to_CL(CONTLOC_CHANNEL, CONTLOC_HOST) ; 

// cout << "Connected to CONTINUOUS LOCALIZATION on" << CONTLOC_HOST 
<< ve a 

Tf << endl 


connect_to_CL(CONTLOC_ CHANNEL, comm_mach) ; 
cout << "Connected Eo CONTINUOUS LOCALIZATION om” << COMmeimadch << aa. 
<< endl; 


Cont loc mode «1; 


} 


void agent: :send cl grid({void) 


{ 


// Send global grid to continuous localization 


iE ee One LOC mode) iat 
return; 


} 


cout <<" Sending Global grid to CONTINUOUS=LOGCALIZATION. << “endl: 
Savenguid rile (gloselvaricd ARIEL Chee tie) 5 


// SCOUT THESIS CHANGE - if continuouse localization is ever used send 
r.theta instead of r.turret 
sendroom_to CL(CONTLOC_ CHANNEL, ARIEL CL FILE, (double) r.x / 10.0, 
(double):<r.y > / -10.0,7- (double, r-ehneta 7 1020, 
(doubler lthetaa,7 -LO0e0, 0. 0.0. Ose, OO) 


2) 


4921 
4922 
4923 
4924 
4925 
4926 
4927 
4928 
4929 
4930 
4931 
4932 
4933 
4934 
4935 
4936 
4937 
4938 
4939 
4940 
4941 
4942 
4943 
4944 
4945 
4946 
4947 
4948 
4949 
4950 
4951 
4952 
4953 
~ 4954 
4955 
4956 
4957 
4958 
4959 
4960 
4961 
4962 
4963 
4964 
4965 
4966 
4967 
4968 
4969 
4970 
497] 
4972 
4973 
4974 
4975 
4976 
4977 
4978 


/* #4 * eK MULTIROBOT COMMUNICATION ****##* HH / 


// BEGIN SCOUT THESIS CHANGE 

// This routine now sets up communication for up to MAX ROBOTS 
simultaneously 

// 2 robot limitation is eliminated 


void agent::init_robot_comm(void) 

{ 
// Initialize robot communication channel 
char robot_server_name [STRLEN]; // Server robot host name 


// I£ Server Robot 


if (r.id == SERVER_ROBOT) { 
if (init _comm_server(ARIEL CHANNEL, PORT _ARIEL, NONBLOCK_COMM) == 0) 
{ 
eout << "init rObOlLscomm Rober << 7 ad 
<< "] initialized communications as server." << endl; 


tule mode — 1; 
} 
else { 
COuk <<. Ina tor obee Comm "RObGE 1) Sa< Nea 
<< "] unable to set up communications as server." << endl; 
Mule. mods -— 0, 
} 
} 
else if (r.id <= MAX ROBOTS) { 
cout << "Enter host name for server robot 
Gin 77 PODOr, Server Mame, 


= tt 
==> : 


if (init _comm_client (ARIEL CHANNEL, robot server name, 
BASE CLIENT PORT + r.id, NONBLOCK_ COMM) == 0) { 
GOuc << Sar robor Comm Reset (<< rad 


<< "] initialized communications as client." << endl; 
Mates mode = 1, 
} 
else { 
6oue <=. “init robot comm: Robet | << x 7xd 
<< "] unable to set up communications as client." << endl; 


multi _mode = 0; 
} 
} 


else { 
Coun << "anit robou scomm., Robot |" “=< 7 -ad 
<< "] unable to set up communications for more than " 


<< MAX ROBOTS 
<< “ Tebors. << endl; 
multi_mode = 0; 
} 
} 
//END SCOUT THESIS CHANGE 


void agent::send_robot_message(char *message) 
{ 


// Send message to other robot 
// BEGIN SCOUT THESIS CHANGE 
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4979 
4980 
4981 
4982 
4983 
4984 
4985 
4986 
4987 
4988 
4989 
4990 
4991 
4992 
4993 
4994 
4995 
4996 
4997 
4998 
4999 
5000 
5001 
5002 
5003 
5004 
5005 
5006 
5007 
5008 
5009 
5010 
5011 
5012 
5013 
5014 
5015 
5016 
BO17 
5018 
5019 
5020 
5021 
5022 
5023 
5024 
5025 
5026 
5027 
5028 
5029 
5030 
5031 
5032 
5033 
5034 
5035 
5036 


<< send > 
send message 


cout << "Sending message <" << message << ">." 
// Leop thru all possible client robots connections, 
// that new map is available. 
for {ant a=i;  i< MAX ROBOTS; 2t4){ 
write comm(i, message, strien(message) + 1); 
} 
//END SCOUT THESIS CHANGE 
} 


void agent::user send robot _message (void) 


{ 


// Send message to other robot (user mode) 


char message[{ STRLEN] ; 


cout << "Enter message ==> "; 
cin >> message; 


cout << "Sending message <" << message << ">." << endl; 


write commi{ARTEL CHANNEL, message, Sstrlen(message) + 1); 


} 


//BEGIN SCOUT THESIS CHANGE 
// Pass in the channel used for communication between client 
// and server 


int agent::receive robot_message(int channel, char *message) 


{ 


// Receive message from other robot 
// Returns 1 if message received, 0 otherwise 


int message received; // Message receipt flag 


message received = read comm(channel, message, STRLEN); 
// END SCOUT THESIS CHANGE 
if message Tecerved) { 
cout << "Received message <" << message << ">." << endl; 


} 


return (message received) ; 


} 


void agent::user receive robot _message(void) 


{ 


// Receive message from other robot (user mode) 


char message[{ STRLEN] ; 


if (read_comm(ARIEL CHANNEL, message, STRLEN) == 0) { 
cout << "No messages waiting." << endl; 
} 
else { 
cout << "Received message <" << message << ">." << endl; 
} 
} 


[RR OK KR Rk Rk kk ke ke ok MULTIROBOT EXPLORATION Kak KKK KK KKK KKK KKK KKK / 
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5037 
5038 
5039 
5040 
5041 
5042 
5043 
5044 
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5046 
5047 
5048 
5049 
5050 
5051 
5052 
5053 
5054 
5055 
5056 
5057 
5058 
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5060 
5061 
5062 
5063 
5064 
5065 
5066 
5067 
5068 
5069 
5070 
5071 
5072 
5073 
5074 
5075 
5076 
5077 
5078 
5079 
5080 
5081 
5082 
5083 
5084 
5085 
5086 
5087 
5088 
5089 
5090 
5091 
5092 
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5094 


void 


{ 


agent::integrate remote _map(void) 
// Integrate new map from remote robot (if a new map exists) 
Map3D remote grid; // Evidence grid for remote map 
char mapfile{ STRLEN ; // Remote map file 


enar posinto, STIRLEN! ; // Remote map position information 
int mx, my, mtheta; // Position of center of new map 
// (1/10 inch, 1/10 degree) 


// BEGIN SCOUT THESIS CHANGE 


int channel; 


// 
es 
// 


// Channel number 

Loop thru all channels corresponding to client robots. Check each 
channel to see if we received a new map message. 

Robot 2 is on channel 1, Robot3 is on channel 2, 


for (channel=1; channel < MAX ROBOTS; channelt+t+){ 


endl; 


// Check for new map message 


if (!receive robot_message(channel, mapfile)) { 
continue; // If nothing to read on this channel, 
// do not give up, continue will jump 
// back to "for" loop and increment 
// channel counter. 


} 


cout << "New map from remote robot in <" << mapfile << ">." << 


// Load Grid along with position into 


tf ((tiead grids tale com éuemoce yarid, 
Fecurn; 


mapfile, posinfo)) { 


} 


wo 


sscanf(posinfo, "Sd td Sd", &mx, &my, &mtheta); 


cout << "New map position = (" << mz << ", “ << my <<" [igre 


mtheta << "j" 


<< end, 
// Display and integrate new map 


// 


grid _display(grid_window, remote grid); 


// NEW MAJOR SCOUT THESIS change below 

// 1 r.id==1 then robot is SERVER and needs to integrate a local scan 
to the global map 

// if r.id !=1 then robot is CLIENt and needs to intgrate the SERVER 
global map that is sent 


af 
integrate _grid(global grid, remote grid, 


} 


(r.id==1) { 
os (double) mx 9 / “120.0, 
(double) my / 120.0, (double) mtheta / 10.0); 


else { 
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5095 
5096 
5097 
5098 
5099 
5100 
5101 
5102 
5103 
5104 
5105 


seal acca remote grid, (double) mx / 
(double) my / 120.0, (double) mtheta / 10.0); 
} // close for else r.id !=1 


grid display global(global_ grid); 
} // close for channel check counter 


// END SCOUT THESIS CHANGE 
j 
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APPENDIX I. FRONTIER-BASED EXPLORATION CODE — COMM.H 


This appendix contains the header file for the communications routine that allows 
multiple robots to send messages to one another. 


/* Original code written by William Adams */ 


/* Modifications for increased number of robots June 1998 
for Master's Thesis work by Patrick A. Hillmeyer =f 


/* include file for comm.c and all files linking to comm.o */ 
enum { OFF COMM, NONBLOCK_COMM, BLOCK _COMM} ; 


#define PORT DETECT 65003 
#define PORT GESTURE 65004 
#define PORT AUXPORT 65005 
#define PORT _CONTSERV 65006 
#define PORT SEARCH 65007 
#define PORT CONTLOC 65008 
#define PORT ARIEL 65009 


/* BEGIN SCOUT THESIS CHANGE */ 


#define MAX ROBOTS 9 /* This is the max number of robots that can 
operate at one time - change as you get 
more robots - this must always be one less 
than MAXCHANNEL - see important note about 
MAXCHANNEL and comm.c file. aay 


#define SERVER ROBOT 1 /* This is the ID number (in Nserver) of the 
robot that will act as the Server robot 
to the other Client robots for receiving and 
sending .eg files */ 


#define BASE CLIENT PORT 65007 


j* SIMPORTANT(') “The 1nitializatzon -of global arraysesd, 1d. sand 

comm mode in the file comm.c has to match EXACTLY with MAXCHANNEL. 
Example: if MAXCHANNEL is 10 then you need 10 zeros to initialize each 
of the arrays mentioned above. no, 


#define MAXCHANNEL MAX ROBOTS + 1 

/* Any single process can communicate with this 
many other processes. Although the port numbers 
on both ends of the communication must agree, 
the channel numbers do not need to agree. 
Within a single process, each communication 
link must have a unique channel number. */ 

/* If this is changed, you must change the initializing 
declaration Using 1t-1nconmse.* / 


/* END SCOUE THESIS CHANGE */7 
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APPENDIX J. FRONTIER-BASED EXPLORATION CODE — COMM.C 


This appendix contains the source code for the communications routine that allows 
multiple robots to send messages to one another. 


PERE EARAEE PARA REE A eS eee eee EE Re eR Ee eR Eee ek ee ee 


7 Som server. c 
written: 11/22/95 William Adams 
last modifed: 1/22/95 William Adams 


Set up internet communication on a single port. 
Receive requests from ONE other process and send back 
information. 


t+ +  F FF F 


Ba nara ee ear ta cree ate aoe eee a eae ee ae ee ee 


/* Modifications for increased number of robots June 1998 
for Master's Thesis work by Patrick A. Hillmeyer aed 


#include <stdio.h> 
#include <sys/types.h> 
#include <sys/socket.h> 
#include <netinet/in.h> 
#include <netdb.h> 
fimedude. <fencel. he 
#include <errno.h> 


#include “comm.h" 
enum status { NOTHING C, HALFWAY C, READY C} ; 


// BEGIN SCOUT THESIS CHANGE 
// *** see notes in comm.h file concerning these next few lines 


int sd[ MAXCHANNEL] = 4°00, 0207-0 Or. 0F OG: /* socket handle */ 
int ld[ MAXCHANNEL] = 407070, O70 00-0 07 ul. 
int comm_mode{ MAXCHANNEL] = {0,0,0,0,0,0,0,0,0,0} ; 


// END SCOUT THESIS CHANGE 
int haveaclient = 0; 


/* watt, Clienteto call) biecking ~/ 
LOULeconme waite touecl one \Vchonmel, conero! 
int=channel, control: 
{ 
if (comm _mode[ channel] == NOTHING C) { 
forme i (Sedern, 
"\nImproper call to comm _wait for client...use 
Itt OCOMlmscer Vem. lm ):, 
return(5); 
} 
else if (comm mode{ channel] == READY C) { 
fpringt (stderr, e 
M\nRedundant call to comm wait for client, ignored\n’ ); 
Seturm( 0); 
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/* else comm_modef{ channel] == HALFWAY _C which is correct */ 
if ((sd{ channel) =accept (ld{[ channel] ,0,0))<0O) { 


perron( INE! Domain Accepe”); 
return(5); 


/* set to non-blocking if specified, else default is blocking */ 
if (control==NONBLOCK COMM) 

fentl(sd[ channel] ,F SETFL,O NDELAY) ; 
comm mode{ channel] = READY _C; /* success */ 


retuzmn(0)> /* suecess *7 


Lit Ini comm server (channel, port num, control) 
int channel,port_num,control; 


// BEGIN SCOUT THESIS CHANGE 


static int num_socs = 0; /* Number of sockets already established. */ 
Pie ee, 

ine ddrlen; 

struct sockaddr_in name; 

Struce sockaddr in ptr, 

Struct sockaddr addr, 

Struct MOstent *~hp, *~qeunestbyaddr()-; 

Tee rs, 


// If you are the SERVER ROBOT, set up next available channel 
// for the new client robot 
if (port num == PORT ARIEL){ 
channel = ++num_socs; 
port num = BASE CLIENT PORT + num_socs + 1; 
} 


// END SCOUT THESIS -CEANGE 


/* create a "listen" socket to receive service requests */ 
if ((ld{ channel] =socket (AF INET,SOCK STREAM, 6))<Q) { 
perror(” INET Demain Secker” )-; 
FetuUEnN (!) +? 


} 


/* initialize fields in an Internet address structure */ 
name-Sinwtamity = (shore tac) Av INET; 

NameeSin pore = Neens (pore mum), 

name.sin_addr.s_ addr = INADDR_ ANY; 


/* bind the Internet address to the Internet socket */ 

if (bind(ld[ channel], (struct sockaddr *) &name,sizeof(name))<0O) { 
close(ld[ channel] ); 
perror("INET Domain Bind"); 
return (2); 
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id /* find out the port number assigned to our socket */ 
1 addrlen = sizeof (addr); 

113 if ((rc=getsockname (1ld{ channel] , &addr, &addrlen))<0) { 
114 perror ("INET Domain getsockname" ) ; 

Niles return (3); 

116 } 

117 

118 /* now “advertise” the port number assigned to us */ 
119 ptr = (struct sockaddr in *) éaddr; 

120 YS printi(” \n\ tsocket Mas pore number: sey 7mibons (pur—>sim: port) jas. = / 
121 

122 /* mark socket as a passive "listen" socket */ 

123 if “(Pisteniied channel) ,5)<0) 4 

124 perror ("INET Domain Listen"); 

125 return (4); 

126 } 

127 

128 /* Waite for a Client to contact us... (piecing) *7 
129 comm mode[ channel] = HALFWAY C; 

130 if ((err=comm_wait for _client(channel,control)) != 0) { 
131 return (err) ; 

2 } 

33 

134 /* find out who's calling us */ 

35 /*if ((rc=getpeername (sd{ channel] , &addr, &addrlen))<0) { 
136 perror ("INET Domain getpeername" ) ; 

137 return (6); 

138 ae 

139 

140 Jj.) announce! “the cativer: = / 

141 /* printf ("\n\tCalling socket from host %s\n",inet_ntoa(ptr->sin addr) ); 
142 erirnte eat has port number %d\n",ptr->sin_ port); 
143 if ((hp=gethostbyaddr (&ptr->sin_addr,4, AF _INET)) != NULL) { 
144 printf ("\tFrom hostname: %s\n\tWith aliases:",hp->h_name) ; 
145 while (*hp->h_ aliases) 

146 printf ("\n\t\t\t%s",*hp->h_ aliases++) ; 

147 prank a Vaca); 

148 } 

149 else { 

150 perror ("\n\tgethostbyaddr() failed"); 

151 promtve (\n\ehverrno is, Sd\ ni nk, necro) 

152 I Ca 

153 

154 comm mode{ channel] = READY C; 

55 return(0); 

156} 

7 

158 

159 


160 int init_comm_client (channel,mach_name,port num, control) 
161 int channel; _ Ps - 

162 char *mach name; 

N63" int port num, control; 


164 { 

165 struct sockaddr in name; 

ee struct hostent *hp, *gethostbyaddr (); 

168 /* create a "Client" socket to request service */ 
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169 if ((sd{ channel] =socket (AF_INET,SOCK STREAM,0))<0) { 


170 perror("INET Domain Socket"); 

171 return (1); 

le2 } 

ls 

174 /* initialize fields in an Internet address structure */ 
is name.sin_ family = AF_INET; 

176 name.sin port = htons (port num); 

177 hp=gethostbyname(mach_name); 

as memcpy (&name.sin_addr.s_ addr,hp->h_addr,hp->h_ length); 
180 if (connect (sd{ channel] , (struct sockaddr *)&name, sizeof (name) )<0) { 
181 perror | Connect () = 

182 return 

183 } 

184 

185 /* set to non-blocking if Specified, else default is blockings = 
186 if (control==NONBLOCK_COM™M) 

187 fentl (sd{ channel] ,F_SETFL,O NDELAY) ; 

188 

189 comm _mode[ channel] = READY C; /* success */ 

190 return (0)7 

191} 

192 

193 

194 

195 int read _comm(channel, buf, bufsize) 


196 int channel; 
197 char *buf; 
198 int bufsize; 


199 { 

200 int nbytes; 

201 

202 // BEGIN SCOUT CHANGE 

203 // If no socket has been established on this channel, 
204 Jj 7 thenereturn. 

205 if (sd{ channel] == 0){ 

206 return(0); 

207 } 

208  $// END SCOUNT CHANGE 

209 

210 if (comm _mode[ channel] == READY C) { 

2 memset (buf,0,bufsize) ; 

212 if ((nbytes=read(sd{ channel] , buf, bufsize))<0) { 
213 if (errno!=EWOULDBLOCK) { 

214 perror("INET domain Read"); 

ZS return(-1); /* indicate error */ 

216 } 

Gy else /* it was just an unblocked read with no data ready */ 
218 return(0); 

219 } 

220 else if (nbytes==0) { 

221 fprintf(stderr,"\nSender Disappeared.\n"); 
222, comm mode{ channel] = HALFWAY C; 

223 return(-2); /* no data to be read */ 

224 } 

225 else { 

226 return(nbytes); /* read data */ 
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227 
228 
229 
230 
231 
232 
233 
234 
235 
236 
2AM 
238 
239 
240 
241 
242 
243 
244 
245 
246 
247 
248 
249 
250 
Z5\ 
D2 
253 
254 
p55 
256 
ZS / 
258 


260 
261 
262 
263 
264 
265 
266 
267 
268 
269 
270 
271 
Pi 
2713 
274 
A 
276 
Zi 
278 


} 
} 


else /* socket has not been initialized */ 


return (—3) > 


void write comm(channel, buf, bufsize) 
int channel; 
enar *bu; 
int bufsize; 
{ 
Ee (comm model channel == READY 7) 
write(sd[ channel] ,buf,bufsize) ; 


demoserver () /* demo */ 
{ 

int a=0; 

int b=10; 

ite = 

char but, 256) ; 


init comm server (0, 65003,NONBLOCK COMM) ; 


while (1) { 
if (read _comm(0,buf,sizeof(buf))>0) { 


/* if read something */ 


Spranel (bul, ta sa sa\0" fat), bt+,c.)-; 


write comm(0,buf,sizeof (buf) ); 
} 
sleep (1); 
} 
} 


democlient () 

{ 
int ayo 7e> 
enar butl 256). 


iMieecoum Glient(0;, coyoue 7650037; PLOCK COMM); 


while (1) { 
Strcpy (bur,  Requcst); 
write comm(0,buf, sizeof (buf) ); 
read comm(0,buf,sizeof (buf) ); 
SSCant (but, 20 sc jsa, ka;,.&b, 6c)? 


printf ("\nreceived $d $d $d\n",a,b,c); 


frlush (stdout): 
sleep(1); 
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